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ABSTRACT 


The  major  problem  addressed  by  this  research  is  how  to  improve  an  existing  real-time 
software  system’s  readability,  maintainability,  stability  and  portability  using  reengineering 
techniques.  A  fundamental  portion  of  the  Model-based  Mobile  robot  Language  (MML) 
was  the  real-time  system  chosen  as  the  basis  for  this  study. 

The  approach  taken  was  to  create  a  new  system  design.  The  new  design  was  based  on 
system  specifications  obtained  by  conducting  static  and  dynamic  analysis  on  the  existing 
system. 

The  results  are  that  a  new  core  system  was  implemented  using  a  design  that  fowuacu  on 
creating  independent  software  sub- systems  while  encapsulating  data.  Hardware 
dependencies  were  localized  and  assembly  code  minimized.  The  new  system  is  easier  to 
understand  and  modify  and  is  portable  to  other  hardware  platforms. 
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I.  INTRODUCTION 


A.  PROBLEM  STATEMENT 

The  problem  this  thesis  solves  is  how  to  reengineer  MML,  a  real-time  control  system 
for  the  Yamabico-1 1  autonomous  robot,  while  improving  system  stability,  maintainability 
and  portability. 

B.  BACKGROUND 

MML  is  a  real-time  system  under  development  for  the  Yamabico-1 1  robot.  The  goal 
of  the  project  is  to  create  a  robot-independent,  high-level  language  for  mobile  robot  control. 
The  language  contains  sets  of  library  functions  to  handle  geometry,  motion,  sonar,  and  I/O. 
These  routines  could  then  be  used  by  developers  to  program  a  robot’s  movement  without 
the  requirements  of  low-level  motion  control  understanding. 

In  the  class  of  motion  control  functionality,  MML  originally  used  a  sequence  of 
configurations  to  describe  a  vehicles  desired  path.  A  configuration  represents  the  robot’s 
current  position  and  orientation  in  a  2D  world.  Current  research  involves  expanding  the 
language  to  describe  motion  control  using  directed  path  segments  as  well.  Each  directed 
path  is  defined  by  a  point  that  lies  on  the  segment,  the  orientation  of  the  segment  in  the  2D 
world  and  the  segment’s  curvature. 

The  status  of  the  MML  system  typifies  that  of  many  software  systems  in  existence 
today.  Roughly  50  to  80  percent  of  a  software  organization’s  resources  are  spent 
maintaining  existing  systems.  Many  of  these  systems  were  developed  without  complete 
specifications  or  documentation  by  analysts  and  programmers  who  are  no  longer  with  the 
organization.  Analysts  and  programmers  with  incomplete  system  knowledge  are  then 
required  to  perform  the  necessary  maintenance  [Y ourdon  93]. 

As  a  research  system,  MML  evolved  through  modifications  made  from  several 
graduate  students.  Many  of  these  changes  were  unstructured  and  made  use  of  global 
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variables.  At  times,  particular  functions  or  sections  of  code  were  altered  simultaneously  by 
different  developers.  The  result  is  an  unstable  system  that  is  extremely  difficult  to  maintain. 

Some  organizations  are  investing  resources  into  software  reengineer  techniques, 
hoping  to  reduce  maintenance  efforts  and  costs  dedicated  to  these  existing  systems.  There 
are  three  fundamental  approaches  to  reengineering  a  system.  The  first  technique,  called 
restructuring,  reorganizes  unstructured  source  code  into  a  modular  form.  The  new  form 
remains  functionally  equivalent  to  the  older  version.  The  second  method  is  to  reengineer 
the  system.  The  goal  of  this  plan  is  to  replace  existing  code  with  newer  versions,  possibly 
written  in  a  higher  level  language.  This  is  done  gradually  by  the  maintenance  programmers 
each  time  maintenance  is  required.  Reverse  engineering  is  the  third  technique.  The 
objective  of  this  procedure  is  to  reconstruct  the  design,  and/or  the  specifications  from  the 
existing  source  code  [Yourdon  93]. 

C.  OVERVIEW 

This  thesis  shows  a  procedure  for  reengineering  an  existing  real-time  system,  MML- 
10.  The  first  step  in  this  process  is  to  determine  the  functionality  of  the  existing  system. 
Chapter  two  analyzes  MML-lO’s  software  system,  while  chapter  three  covers  the  hardware 
components  of  the  Yamabico-1 1  robot.  Generating  a  new  design  is  the  second  step  and  is 
covered  in  chapter  four.  The  third  step  is  to  implement  and  test  the  new  design.  Chapter  five 
details  Yamah^o-l l’s  software  development  environment.  The  design  implementation 
and  results  are  discussed  in  chapter  six.  Chapter  seven  presents  the  conclusions  and  future 
recommendations. 
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II.  YAM  *  3ICO  SOFTWARE  SYSTEM  ARCHITECTURE 


Prior  to  reengineering  an  existing  software  system,  the  maintenance  programmer  must 
have  a  thorough  understanding  of  that  system’s  logical  design.  Unless  the  programmer  was 
involved  with  the  system’s  original  implementation,  this  is  only  achieved  by  reviewing  the 
software’s  specifications  and/or  documentation.  For  systems  lacking  complete 
specifications  and  /or  documentation,  such  as  MML,  it  may  be  necessary  to  perform  reverse 
engineering  techniques  to  recover  this  information.  Since  current  CASE  technology 
focuses  on  developing  new  systems,  these  tools  can  provide  limited  support,  but  not  at  a 
high  level  [Yourdon  93]. 

Two  of  the  most  effective  methods  for  reconstructing  system  specifications  and/or 
documentation  are  static  and  dynamic  analysis.  The  results  of  these  two  techniques  form  a 
logical  description  of  the  system.  This  description  can  then  be  translated  into  physical 
depictions,  such  as  data  flow  diagrams  or  state  transition  diagrams  fYourdon  89],  The 
context  diagram  of  the  current  MML  system  is  shown  in  Figure  1. 


Figure  1:  MML  System  Overview 
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A.  STATIC  ANALYSIS  OF  MML-10 


Static  analysis  is  the  process  of  gaining  an  understanding  of  a  software  system  through 
source  code  examination.  The  goal  of  this  phase  is  to  recover  enough  information  to  create 
a  logical  picture  of  the  existing  system.  One  important  technique  involves  the  tracing  of 
function  and  procedure  calls.  When  performing  this  procedure,  it  is  not  desirable  to  produce 
a  complete  or  detailed  trace  as  this  only  wastes  time  and  resources. 

Another  important  technique  involves  variable  tracing.  The  emphasis  during  this 
operation  should  be  on  tracing  parameters  passed  to  functions  and  procedures,  any  values 
they  return  and  references  to  global  variables. 

1.  User  vs.  Kernel 

As  shown  in  Figure  2,  the  MML  system  is  composed  of  two  distinct  parts:  the 
kernel  module  and  a  user  program.  The  kernel  contains  the  low  level  routines  required  to 

Wheel 

Encoders 


Output  Signals 
Figure  2:  MML  System 

control  movement  and  to  perform  input/output.  The  low  level  details  are  hidden  from 
developers  through  the  MML  command  set.  A  user  program  consists  of  MML  command 
calls  and  C  constructs  to  describe  one  or  more  motion  behaviors.  Each  command  either 
requests  status  information  from  the  kernel  or  commands  a  desired  behavior  fScott  93]. 
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Each  module  is  downloaded  to  the  robot  separately.  Robot  operation  is  only 
possible  after  both  modules  are  loaded.  Program  execution  starts  with  the  kernel.  The 
kernel  initializes  the  sub-systems  and  then  passes  control  to  the  user  program.  The  user 
program  maintains  primary  control  until  termination.  However,  the  kernel’s  control 
processes  will  interrupt  execution  for  short  durations.  This  design  allows  a  user  to  quickly 
alter  the  behavior  of  the  robot  by  simply  changing  the  user  program  used  by  the  kernel. 

2.  Kernel  System 

Figure  3  is  a  modified  data  flow  diagram  showing  the  kernel  as  a  collection  of 
control  type  processes.  Each  control  process,  indicated  by  a  dashed  circle,  is  associated 
with  a  distinct  hardware  generated  interrupt.  The  interrupts  are  prioritized  through  the 
hardware’s  configuration.  The  current  CPU  provides  eight  priority  levels.  When  an 
interrupt  is  generated,  the  associated  control  process  assumes  control  of  the  CPU,  provided 
it  is  not  interrupting  a  higher  level  process.  Otherwise,  it  will  wait  until  all  higher  priority 
interrupts  are  serviced  before  taking  control.  The  interrupt  level  of  each  process  is  indicated 
by  the  value  within  parentheses. 

Since  motion  control  is  responsible  for  maneuvering  the  robot,  receives  the 
highest  priority  given  to  the  control  processes,  interrupt  level  four.  It  controls  movement 
by  first  estimating  the  robot’s  current  odometry  configuration  through  dead  reckoning.  This 
estimate  is  then  used  to  calculate  the  necessary  linear  and  rotational  velocities.  These 
calculations  are  based  on  the  motion  required  to  follow  the  current  path  segment.  Finally, 
these  velocities  are  translated  into  pulse  width  modulation  commands  and  sent  to  the 
motors  controlling  the  robot’s  wheels.  This  process,  called  the  motion  control  cycle,  repeats 
every  10  milliseconds  and  requires  approximately  2.5  milliseconds  to  execute 
[MacPherson  93]. 

The  input/output  process  receives  the  next  highest  priority  at  interrupt  level 
three.  It  uses  interrupt  driven  input  and  output  to  effect  information  transfers  between  the 
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Figure  3:  Kernel  System 
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robot  and  the  onboard  console  device.  It  also  controls  the  transfer  of  data  between  the  robot 
and  a  Unix  workstation  [MacPherson  93]. 

Interrupt  priority  level  two  signals  the  robot’s  sonar  process.  This  process 
collects  sonar  range  information  used  for  obstacle  avoidance.  The  process  requires  240 
microseconds  to  complete  and  is  repeated  every  24  milliseconds  [DeClue  93]. 

3.  Motion  Control  Process 

As  depicted  in  Figure  4,  the  major  responsibilities  of  motion  control  are 
processing  MML  commands  and  pathtracking.  Each  MML  command  is  characterized  as 


Interrupt 
from  Serial 
Board 


Wheel 

Encoders 


Global  Variables 


Figure  4:  Motion  Control  Process 


either  an  immediate  or  sequential  command.  Both  types  induce  changes  to  one  or  more 
control  parameters.  Immediate  commands  induce  the  changes  at  the  instant  they  are  called 
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from  within  the  user  program.  However,  when  a  sequential  command  is  called  from  the 
user  program,  it  is  added  to  an  instruction  queue.  Each  sequential  command  is  then 
executed  only  after  the  previous  sequential  command  is  finished.  Immediate  command 
execution  and  storage  of  sequential  commands  into  the  instruction  queue  is  independent 
from  the  motion  control  cycle. 

For  path  tracking,  a  three  step  process  is  executed  every  motion  control  cycle. 
First,  Yamabico  updates  its  current  odometry  configuration,  position  and  orientation,  by 
reading  hardware  registers  associated  with  wheel  movement  Next,  this  information  is  used 
to  calculate  the  robot’s  next  intended  movement  using  control  rules.  Early  motion 
description  methods  in  MML  compared  the  current  configuration  against  a  reference 
configuration  [Kanayama  91].  However,  recent  research  has  proven  that  smoother  motion 
control  is  achieved  by  comparing  the  current  configuration  against  a  reference  path 
segment  [MacPherson  93].  Finally,  the  intended  movement  is  translated  into  pulse  width 
modulation  values  and  sent  to  the  motor  control  board. 

A  path  segment  is  either  a  straight  line,  circular  arc,  parabola  or  cubic  spiral. 
Yamabico  will  track  a  sequence  of  these  path  segments  added  to  the  instruction  queue  by 
calculating  a  transition  point  from  one  segment  to  the  next.  Once  the  transition  point  is 
reached,  the  robot  will  begin  following  the  next  path  segment 

A  secondary  task  of  motion  control  is  data  logging.  This  purpose  of  this  function 
is  to  record  control  data  every  motion  control  cycle.  The  collected  data  is  used  by  the 
system  programmers  for  debugging. 

4.  I/O  Process 

As  previously  mentioned,  the  input/output  process  is  responsible  for  transferring 
information  between  the  robot  and  either  the  Unix  workstation  or  the  console. 
Communications  between  the  robot  and  the  Unix  workstation  are  accomplished  by  polling 
a  serial  port  mapped  to  a  specific  memory  location. 
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Transferring  information  with  the  console  is  also  memory  mapped,  however  this 
process  is  interrupt  driven.  During  output,  data  is  stored  in  a  1024  character  circular  buffer. 
The  interrupt  sequence  is  then  initiated  by  sending  a  null  character  directly  to  the  console. 
After  the  character  is  received  by  the  console,  an  interrupt  causes  the  next  character  in  the 
buffer  to  be  sent.  After  this  character  is  written  to  the  console,  another  interrupt  is 
generated.  This  process  continues  until  the  buffer  is  empty  and  a  command  to  terminate  the 
interrupt  cycle  is  sent  to  the  port.  Input  is  handled  in  a  similar  fashion. 

5.  Sonar  Process 

As  mentioned  earlier,  the  sonar  process  is  responsible  for  recording  sonar  range 
returns.  This  information  is  made  available  to  user  programs  for  obstacle  avoidance.  Th" 
returns  can  also  be  used  for  automated  cartography  by  building  line  segments  using  a  least 
squares  linear  fitting  algorithm  [DeClue  93][MacPherson  93]. 

B.  DYNAMIC  ANALYSIS  OF  MML-10 

When  static  analysis  provides  insufficient  insight  to  a  system’s  behavior,  dynamic 
analysis  should  be  used.  Dynamic  analysis  is  the  process  of  tracing  a  system  during 
execution.  Using  dynamic  analysis,  a  maintenance  programmer  can  follow  the  path  of 
execution,  monitor  access  to  a  particular  memory  location  or  alter  the  storage  values.  This 
is  particularly  useful  when  trying  to  locate  the  cause  of  a  system  crash  [Yourdon  93]. 

Tracing  MML-10  has  proven  to  be  very  difficult.  Two  reasons  are  the  system’s  low 
readability  and  high  degree  of  coupling.  This  is  the  result  of  using  inappropriate  modularity 
techniques.  These  and  other  system  characteristics  are  described  below.  Another  reason  is 
the  difficulty  associated  with  using  a  debugger  due  to  the  timing  constraints  of  a  real  time 
system. 

C.  SYSTEM  CHARACTERISTICS 

A  by-product  of  conducting  static  and  dynamic  analysis  is  knowledge  of  a  system’s 
software  characteristics.  Software  characteristics  are  those  attributes  used  to  describe  the 
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quality  of  a  system’s  design  and  implementation.  The  following  list  of  attributes  is  used  to 
describe  the  internal  makeup  of  a  system  and  is  directly  related  to  the  effort  required  to 
perform  system  maintenance. 

1.  Coupling 

When  one  module  references  a  symbolic  address  defined  outside  of  that  module, 
a  connection  (or  interdependency)  is  created  between  the  module  with  the  reference  and  the 
module  with  the  definition.  Coupling  describes  the  types  and  strength  of  these  connections 
between  modules.  References  to  internal  data  elements  or  data  structures  is  known  as 
common  coupling  since  code  segments  are  referencing  a  common  data  area.  Another  form 
of  coupling  is  control  coupling.  This  exists  when  control  switches  such  as  flags  are  used 
between  modules.  The  purpose  of  these  flags  is  to  change  or  modify  the  behavior  or  actions 
of  a  routine.  Low  coupling  exists  when  references  between  modules  are  limited  to 
procedure  and/or  function  names. 

As  coupling  increases,  a  system  is  more  difficult  to  understand  and  maintain. 
Therefore,  it  is  desirable  to  reduce  coupling  by  reducing  the  references  to  another  module’s 
internal  elements.  One  method  for  reducing  common  coupling  is  to  bring  the  externally 
referenced  elements  inside  the  module.  However,  this  only  works  if  the  elements  are  not 
referenced  by  other  sections  of  code.  Control  coupling  can  be  reduced  by  splitting  the 
routines  effected  by  the  flags  into  seperate  procedures  or  functions.  Then  the  calling  routine 
would  make  seperate  calls  to  the  new  routines  [Stevens  74]. 

2.  Cohesion 

When  more  than  one  code  segment  references  the  same  element,  these  segments 
are  related.  Cohesion  measures  the  strength  of  relationships  between  code  segments  within 
the  same  module.  It  is  desirable  that  modules  exhibit  strong  cohesion.  For  that  reason, 
related  segments  should  be  collected  in  the  same  module  that  contains  the  referenced 
element  [Stevens  74]. 
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3.  Modifiability 

A  system  is  modifiable  if  changes  can  be  made  to  one  segment  of  code  without 
generating  adverse  side  effects  in  another  segment.  Another  name  for  this  attribute  is 
stability.  The  degree  that  a  system  is  modifiable  is  directly  related  to  the  system’s  measure 
of  coupling  and  cohesiveness.  A  modifiable  system  is  produced  through  the  application  of 
sound  implementation  techniques  to  a  solid  design  [Yourdon  80].  Once  produced,  a 
modifiable  system  is  easily  changed  and  maintained. 

4.  Modularity 

Modularity  is  defined  as  the  partitioning  of  the  system  into  small  segments. 
Creating  a  modular  system  also  begins  during  the  design  phase.  A  major  goal  of  this 
process  is  to  design  each  segment  around  a  particular  logical  function  performed  by  the 
system  [Pamas  79].  This  produces  a  system  exhibiting  strong  cohesion.  Another  goal  is  to 
minimize  the  amount  of  coupling.  This  is  done  by  using  a  clean  and  concise  interface  with 
data  encapsulation;  the  hiding  of  data  elements.  The  success  of  this  process  is  measured 
through  the  ease  of  implementation  and  maintenance. 

5.  Readability 

Occasionally,  original  developers  are  no  longer  available  after  a  system’s 
completion  [Yourdon  80].  For  this  reason,  systems  need  to  exhibit  the  same  behavior 
during  operation  as  expressed  in  the  source  code.  This  characteristic  is  termed  readability. 
Small  modules  with  independent,  well  defined  and  clearly  documented  behavior  are  the 
most  readable.  Therefore  it  is  important  for  modules  to  exhibit  simplicity  and  consistency 
[Scott  93]. 

6.  Robustness 

Systems  that  can  detect  errors  (or  exceptions)  and  recover  are  considered  robust 
or  fault  tolerant.  This  requires  the  addition  of  exception  handling  functions  and  procedures 
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to  the  system.  These  code  segments  allow  the  system  to  process  exceptional  conditions 
such  as  division  by  zero  or  value  out  of  limits  [Scott  93]. 

D.  CHARACTERISTICS  OF  MML-10 

MML-10  was  an  attempt  to  restructure  the  system  by  collecting  related  functions  into 
seperate  modules.  Although  this  strengthened  cohesion,  improvements  were  limited  due  to 
the  usage  of  global  variables  [Scott  93].  Almost  all  of  the  global  variables  are  declared  and 
initialized  in  the  module  main.c.  However,  most  global  variables  are  actually  used  or 
referenced  in  one  or  more  different  modules.  There  are  also  several  instances  in  MML-10 
where  global  flags  set  in  one  module  effect  the  behavior  of  a  function  in  another.  Collecting 
the  data  elements  into  one  common  area  also  eliminated  the  requirements  for  clean 
interfaces  between  modules.  This  has  resulted  in  a  tightly  coupled  system. 

To  reduce  system  coupling,  the  required  global  variables  should  be  re-located  to  the 
modules  that  reference  them.  These  variables  should  then  be  encapsulated  with  the 
development  of  module  interfaces. 

The  MML-10  source  code  is  difficult  to  read.  One  reason  is  poor  documentation. 
MML-lO’s  comments  are  minimal  and  often  give  incomplete  descriptions  of  the  code. 
Also,  the  short  motion  control  theory  nomenclature  is  used  for  the  system’s  global  variable 
names.  Source  code  documentation  can  be  improved  using  a  combination  of  well  placed, 
informative  comments  and  descriptive  symbolic  names. 

Improper  use  of  pointers  is  another  reason  MML-10  tends  to  be  confusing  and  difficult 
to  understand.  In  many  places,  global  pointers  reference  global  variables  while  some 
assignments  are  improperly  type  casted.  In  other  places,  pointers  are  used  in  a  cryptic 
manner.  For  example,  instead  of  using  standard  indexing  to  sequence  through  an  array, 
pointers  are  used.  These  practices  should  be  avoided. 

One  of  the  goals  of  the  MML  project  was  to  modify  the  system,  making  it  easily  ported 
to  other  hardware  platforms.  This  involved  converting  the  current  system  to  a  portable 
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language,  such  as  ANSI  C  or  C++,  while  minimizing  the  amount  of  required  assembly 
code.  It  also  implies  that  hardware  dependencies  should  be  local  to  a  few  modules. 

However,  attempts  to  reengineer  MML-10  by  replacing  existing  non- ANSI  C  or 
assembly  code  with  ANSI  C  faded.  For  example.  Figure  5  shows  the  addition  of  two 

Module  main.asm.s 


.long  SEAN 

movl  #0  SEAN 


Figure  5:  Introduction  of  a  Unique  Variable 

statements  to  an  assembly  language  module  belonging  to  the  system.  The  first  statement, 
.long  SEAN,  simply  introduces  a  unique  variable,  while  the  second  statement,  movl 
HO, SEAN,  initializes  it  Prior  to  the  change,  the  system  executed  properly.  After  adding  the 
two  program  statements,  the  resulting  system  would  no  longer  function.  No  other  changes 
were  made.  Similar  results  occurred  when  adding  or  deleting  output  statements.  To  create 
a  stable  system,  MML  should  be  re-implemented  using  solid  engineering  techniques. 
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III.  YAMABICO  HARDWARE  ARCHITECTURE 


The  Yamabico-1 1  is  a  collection  of  hardware  sub-systems  assembled  on  an  aluminum 
frame.  As  shown  in  Figure  6,  these  hardware  sub-systems  include:  a  CPU  system,  a  wheels 


Figure  6:  Yamabico-11  Computer  Architecture 


system,  a  sonar  system,  a  serial  system,  a  Unix  host  system  and  a  console  system  used  as 
the  communications  interface. 

The  chassis  houses  two  12-volt  rechargeable  batteries.  These  batteries  power  all  sub¬ 
systems  except  for  the  console.  The  chassis  rests  on  four  spring-loaded  castering  wheels  for 
balance  while  two  wheels  attached  to  the  chassis  in  a  differential  arrangement  control  robot 
movement  The  actual  robot  is  shown  in  Figure  7. 
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Figure  7:  Yamabieo-11  Mobile  Robot 


15 


A.  THE  CPU  SYSTEM 


The  current  CPU  sub-system  uses  a  VME7120  32-bit  motherboard.  The  board  contains 
a  Motorola  based  MC68020  CPU  operating  at  16MHz  and  a  MC6888 1  co-processor  for 
floating  point  arithmetic  [VCM  86].  The  system  also  includes  one  megabyte  of  dynamic 
memory  and  a  ROM  based  VME7920  Debugging  Package.  The  debugger  is  used  as  the 
monitor  program  when  the  system  is  powered  on  [VDP  86].  An  upgrade  of  the  sub- system 
to  a  SPARC-4  processor  board  containing  16  megabytes  of  dynamic  memory  is  planned. 

One  of  the  main  functions  of  the  CPU  is  to  manage  the  interrupt  driven  requests  from 
the  other  subsystems.  As  mentioned  earlier,  the  MC68020  CPU  provides  eight  levels  of 
interrupts.  The  assignment  of  each  interrupt  level  is  summarized  in  Table  1  [MacPherson 
93].  To  handle  the  interrupts,  the  MC68020  uses  a  special  purpose  register  to  hold  the 


TABLE  1:  MML  SYSTEM  TASK  PRIORITY 


Interrupt 

Level 

Interrupt 

Source 

Function 

Interrupt  Type 

Vector 

Duration 

(Its) 

7 

Stop  Button 

Reset 

Asynchronous 

- 

- 

6 

- 

Not  Used 

- 

- 

- 

5 

- 

Not  Used 

- 

- 

- 

4 

Serial  Board  1 

Motion 

Synchronous 

64 

2500 

3 

Serial  Board  0 

Console 

Asynchronous 

65 

variable 

2 

Sonar  Board 

Sonar 

Synchronous 

66 

240 

1 

Serial  Board  0 

Debugger 

Synchronous 

67 

- 

0 

- 

User  Program 

None 

- 

- 

address  of  a  vector  table.  This  table  is  essentially  an  array  of  256  elements,  where  each 
element  can  store  the  address  of  an  exception/interrupt  handler.  When  an  exception/ 
interrupt  is  signaled,  the  CPU  saves  the  current  status  word  and  the  status  word  is  modified 
for  interrupt  processing.  Next,  an  index  value  into  the  vector  table  is  obtained  from  the 
interrupting  device.  Then  the  current  context  is  saved  on  a  supervisor  stack.  Finally, 
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execution  resumes  at  the  address  located  in  the  vector  table  cell  specified  by  the  index 
value.  When  the  interrupt  handler  is  finished,  the  rte  assembly  instruction  restores  the 
processor  to  the  state  prior  to  the  interrupt  [MC68020  85]. 


B.  THE  SERIAL  SYSTEM 

The  serial  sub-system  is  composed  of  two  VME8300  Quad  Serial  Port  Boards.  Each 
board  contains  a  VME  bus  interface  and  two  serial  communications  controllers.  Each 
controller  manages  two  serial  ports.  Associated  with  each  port  is  a  timer/counter  that  may 
be  used  for  baud  rate  generation  or  asynchronous/synchronous  interrupt  control.  There  also 
exists  a  fifth  timer/counter  that  is  primarily  used  to  generate  synchronous  interrupts. 
However,  each  board  contains  only  two  latches  for  a  maximum  configuration  of  two  levels 
of  interrupts  per  board  [VQS  86],  Figure  8  shows  the  conceptual  layout  of  a  serial  board. 


Latch  1 

Timer  #5 

Latch  2 

Controller  1 

Controller  2 

Port  A 

Port  B 

Port  A 

Port  B 

i 

_ 1 

Figure  8:  Serial  Board  Conceptual  Diagram 

The  ports  and  timers  are  configured  by  reading  from  and  writing  to  registers  located 
on  the  serial  boards.  These  registers  are  memory  mapped  and  are  accessed  through  absolute 
addressing. 

C,  THE  WHEELS  SYSTEM 

The  wheel  sub-system  consists  of  two  independent  DC  motors  that  drive  the  two 
wheels  in  either  the  forward  or  reverse  direction,  controlling  the  robot’s  movement. 
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Braking  can  also  be  applied  to  the  motors.  Each  motor  has  an  associated  shaft  encoder  that 
is  used  to  determine  distance  traveled,  speed,  and  to  make  odometry  correction. 

A  dual  axis  controller  board  serves  as  the  interface  between  the  wheel  motors,  the  shaft 
encoders  and  the  CPU.  It  contains  memory  mapped  registers  that  are  accessed  through 
absolute  addressing.  These  registers  are  used  to  enable/disable  the  motors,  read  the  shaft 
encoders  and  to  send  pulse  width  modulation  values.  A  pulse  width  modulation  value  is  an 
eight-bit  integer  that  determines  the  strength  of  a  short  electrical  pulse  sent  to  a  motor  to 
create  movement.  Since  each  pulse  is  short,  constant  pulse  generations  are  required  to 
produce  smooth,  continuous  motion. 

D.  THE  SONAR  SYSTEM 

The  sonar  sub-system  contains  twelve  40kHz  ultrasonic  sensors.  These  sensors  are 
used  to  gather  sonar  return  information  from  the  forward/rear,  lateral  or  diagonal 
directions.  Three  control  boards  are  used  to  control  the  sensors  and  collect  return 
information.  A  VME  bus  card  is  used  as  the  interface  between  the  control  cards  and  the 
CPU  [DeClue  93], 

E.  THE  UNIX  HOST  SYSTEM 

The  Unix  host  system  is  a  Sun-3  workstation  using  SunOS  4. 1.1.  It  is  connected  to  the 
robot  through  a  serial  port  on  the  first  serial  board.  The  host  is  used  to  develop  software  that 
will  be  transferred  to  the  robot.  It  also  accepts  collected  data  from  the  robot.  The  interface 
is  easily  disconnected  during  robot  operation,  allowing  full  motion  freedom. 

F.  THE  CONSOLE  SYSTEM 

A  Macintosh  Power  Book  is  the  main  component  of  the  console  sub-system  and  is  the 
only  input/output  device  when  Yamabico  operates  as  a  self-contained  robot  The  Power 
Book  is  connected  to  the  robot  through  a  serial  port  on  the  second  serial  board  and  provides 
the  interface  between  the  user  and  the  robot’s  debug  monitor  through  a  software 
communications  package.  It  also  has  its  own  rechargeable  power  supply. 
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IV.  SYSTEM  DESIGN 


A.  DESIGN  GOALS 

Initially,  the  goal  of  the  reengineering  process  was  to  add  structure  to  MML-10, 
replacing  non-ANSI  C  code  with  ANSI  C.  However,  it  was  soon  realized  that  a 
monumental  effort  would  be  required.  Therefore,  a  plan  to  re-design  the  core  system  was 
initiated. 

Since  MML  is  used  in  conducting  research  by  many  people,  the  new  design  was  based 
on  three  requirements.  First,  the  system  must  be  easy  to  read  and  easy  to  maintain. 
Therefore,  modularity  is  a  primary  goal  of  the  new  design.  As  discussed  earlier,  a  modular 
system  exhibits  strong  cohesion  and  loose  coupling. 

MML  will  see  many  modifications  and  changes  as  the  system  continues  to  evolve  and 
expand.  Therefore,  the  second  requirement  is  system  stability.  The  creation  of  a  stable 
system  begins  by  applying  solid  implementation  techniques  to  a  modular  design.  Some 
important  techniques  that  promote  stability  are  discussed  in  Chapter  VI. 

The  third  requirement  is  portability.  When  the  Yamabico’s  processor  is  upgraded,  the 
new  software  should  only  require  minor  changes.  This  implies  that  the  assembly  code 
should  be  minimized  and  the  hardware  dependencies  localized.  A  portable  system  also 
promotes  software  re-use  for  future  platforms. 

A  secondary  objective  is  to  create  an  object  oriented  type  design.  This  will  encourage 
the  smooth  transition  to  an  object  oriented  language  such  as  C++. 

B.  MODELING  NOTATION 

When  modeling  software,  the  goal  of  the  notation  is  to  produce  a  picture  of  the 
software  system  that  is  clear  and  easy  to  interpret.  It  must  simplify  the  system  by 
highlighting  the  important  features,  while  hiding  the  details  [Constantine  94]. 

The  notation  chosen  to  model  the  new  design  is  shown  in  Figure  9.  A  software  system 
or  hardware  component  is  represented  by  a  solid  box.  Interrupt  handlers  are  shown  as 
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dashed  circles  while  solid  circles  indicate  system  processes.  In  some  systems,  the  interface 
is  not  yet  finalized.  For  this  reason  a  single  solid  circle  may  represent  more  than  one 
process.  Solid  arrows  depict  messages  between  systems  and/or  processes,  while  dashed 
arrows  display  the  signals  used  to  activate  interrupt  handlers  and  system  control  processes. 
Finally,  data  stores  are  indicated  by  two  solid  parallel  lines. 


System/Hardware 

Component 


System/Process 

Messages 


One  or  More 
Processes 


Interrupt  Handler 


Control  Signal 


Data  Store 


Figure  9:  Design  Notation 


C.  SYSTEM  DESIGN 

1.  System  Overview 

A  mod  '1  of  MML  is  illustrated  in  Figure  10.  This  diagram  depicts  MML  as  a 
network  of  five  sub-systems:  a  CPU  system,  a  motion  control  system,  a  terminal  system,  a 
sonar  system  and  a  user  program.  This  overview  is  slightly  more  complex  than  the  views 
depicted  in  Figure  1  and  Figure  2.  However,  this  approach  helps  produce  the  desired  object 
oriented  design  where  each  sub-system  is  treated  as  an  object.  This  approach  results  in  a 
simpler  system  design  overall. 

2.  CPU  System 

The  CPU  system,  shown  in  Figure  11,  is  the  primary  system.  One  objective  of 
this  system  is  to  initialize  the  other  sub-systems.  If  the  system  is  implemented  with  an 
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Figure  10:  System  Network 


object  oriented  language,  C++  for  example,  this  can  be  accomplished  when  the  object  is 
instantiated.  However,  if  a  non-object  oriented  language  is  used,  such  as  ANSI  C,  then  each 
sub-system  is  required  to  have  an  initialization  process.  In  the  later  case,  the  CPU  system 
must  specifically  call  these  initialization  routines  for  each  sub-system. 

Since  the  interrupt  control  mechanism  is  CPU  dependent,  the  interrupt  handling 
routines  and  the  mechanism  setup  is  part  of  the  CPU  system.  The  objective  of  this  operation 
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Motion 

Control 


Signal 

Output 

Control 


Signal 

Sonar 

Control 


Figure  11:  CPU  System 


is  to  match  the  interrupt  handling  functions  to  the  appropriate  interrupt  signals.  However, 
the  interrupt  priority  levels  are  still  based  on  the  hardware  configuration. 

The  third  function  of  the  CPU  system  is  to  pass  control  to  the  user  program.  This 
is  done  after  the  sub-systems  have  been  initialized  and  the  interrupt  control  process  has 
been  properly  established. 

3.  Serial  System 

Illustrated  in  Figure  12,  the  serial  system  shields  the  hardware  details  of  the 
VME8300  serial  boards  from  the  other  sub-systems  in  MML.  Each  serial  port  or  timer  must 
be  properly  initialized  prior  to  its  first  use.  This  is  accomplished  through  a  pair  of 
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initialization  processes.  A  set  of  interface  routines  are  required  by  each  port  to  read  and 
write  character  data.  These  interface  routines  are  modeled  after  examples  found  in  the 
user’s  manual  [VQS  86].  Since  this  system  directly  accesses  the  hardware,  it  must  NOT  be 
optimized.  If  optimization  techniques  are  used,  the  initialization  routines  will  improperly 
setup  the  ports. 


♦ 


Level  3 
Interrupt 


t 

Level  4 
Interrupt 


Figure  12:  Serial  System 


The  VME8300  boards  ability  to  generate  synchronous  interrupts  is  i  najor 
benefit.  They  are  required  to  generate  control  signals  to  activate  other  sub-systems.  For  this 
reason  the  serial  system  is  considered  to  be  a  component  of  the  CPU  system  rather  than  an 
MML  sub-system  (see  Figure  11). 
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4.  Motion  Control  System 

The  motion  control  system  depicted  in  Figure  13  serves  two  primary  purposes. 
First,  it  provides  the  interface  for  MML’s  immediate  and  sequential  commands.  Immediate 
commands  cause  instantaneous  changes  to  control  variables  and  are  implemented  using  a 
single  process.  A  sequential  command  is  only  executed  after  the  robot  has  completed  the 
previous  sequential  command.  Therefore,  each  sequential  command  requires  a  process 
pair.  One  process,  called  from  the  user  program,  stores  the  command  in  the  instruction 
buffer.  The  other  process  executes  the  command  and  is  called  from  the  motion  system 
control  process. 


Motion 

MML  Commands/  Control 

Status  Requests  Signal 


Figure  13:  Motion  Control  System 


The  system’s  second  purpose  is  to  control  the  robot’s  movement.  This  is  done 
by  executing  the  motion  system  control  process  every  10  milliseconds.  Each  motion  control 
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cycle  begins  by  updating  the  current  configuration  using  information  obtained  from  the 
wheel  system.  Using  motion  control  parameters  and  the  new  configuration,  it  then 
calculates  the  desired  linear  and  rotational  velocities  needed  to  follow  the  current  path 
element.  These  desired  velocities  are  then  sent  back  to  the  wheel  system  for  execution. 

5.  Wheel  System 

As  indicated  by  Figure  13,  the  wheel  system  is  a  component  of  the  motion 
control  system.  Presented  in  Figure  14,  the  system’s  primary  function  is  to  provide  an 
interface  between  the  motion  control  system  and  the  dual  axis  controller.  This  interface  was 
designed  to  eliminate  knowledge  about  the  robot’s  architecture  from  the  motion  control 
system.  Therefore,  the  wheel  system  can  easily  be  replaced  by  another  type  of  locomotion 
system. 


6.  Sonar  System 

As  seen  in  Figure  15,  the  sonar  system’s  design  is  similar  to  the  motion  control 
system.  It  provides  the  MML  sonar  command  interface  between  the  user  program  and  the 
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Figure  15:  Sonar  System 


sonar  board.  Some  commands  control  the  boards  operation  by  setting  control  variables, 
while  others  return  sonar  information  to  the  user  program.  It  also  contains  the  sonar  system 
control  process.  This  process  saves  the  current  sonar  returns  that  may  be  requested  by  the 
user  program.  It  can  also  record  the  sonar  returns  by  sending  the  data  to  the  sonar  trace 
system. 

7.  Tracing  Systems 

A  trace  system  is  a  small  sub-system  used  to  record  selected  information.  Shown 
in  Figure  16,  its  interface  consists  of  three  parts.  First,  it  contains  control  routines  to 
initialize  and  enable/disable  the  system.  The  initialization  routine  sets  the  size  of  the 
logging  buffer,  while  the  enable  routine  sets  the  frequency  that  the  data  is  logged.  The 
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second  part  of  the  interface  consists  of  the  logging  routines.  These  routines  save  the 
selected  data  after  ensuring  that  the  buffer  is  not  full.  The  third  component  downloads  the 
recorded  information  to  the  host  Unix  system.  Only  character  data  can  be  sent  to  the  host 
system.  Therefore,  the  download  process  must  convert  the  data  if  necessary. 

There  are  two  tracing  system  components  in  the  new  design.  One  is  located  in 
the  motion  control  system  and  is  used  for  debugging.  The  other  is  located  in  the  sonar 
system.  This  component  is  used  to  log  sonar  return  information. 


Trace  System 

Log  Data 

Download 

Commands 

Commands 

Command 

▼  cu  uiuivj ,  y 

Logged  Data  Host  Output 


Figure  16:  Tracing  Systems 


8.  Terminal  System 

The  transfer  of  information  between  a  processor  and  a  device,  such  as  a  terminal, 
is  time  consuming.  In  real-time  systems,  it  is  unacceptable  for  a  high  priority  system  to  wait 
for  I/O  completion,  monopolizing  the  CPU.  However,  the  ability  to  display  information 
from  these  high  priority  systems  is  still  desirable. 

1  The  purpose  of  the  terminal  system  is  to  provide  any  sub-system  the  capability 
to  display  information.  As  presented  in  Figure  17,  the  terminal  system  accomplishes  this 


27 


objective  using  asynchronous  interrupt  driven  output  along  with  a  set  of  I/O  routines.  This 
allows  a  high  priority  sub-system  to  continue  executing  by  sending  output  information  to 
the  terminal  system  for  processing.  Input  is  not  interrupt  driven  and  should  only  be  used  by 
low  priority  routines. 


Commands  Output 

Control 

Input  Output  Signal 


Figure  17:  Terminal  System 


When  an  output  routine  is  called,  the  routine  converts  the  data  into  a  character 
representation  if  necessary.  It  then  stores  the  string  of  characters  into  an  output  buffer  that 
uses  a  queue  data  structure.  The  routine  then  sends  a  null  character  directly  to  the  terminal 
using  console  output.  After  a  character  is  sent  to  the  terminal,  an  interrupt  is  generated  by 
the  hardware  requesting  another  character.  This  interrupt  activates  the  terminal  output 
control  process.  The  process  de-queucs  the  first  character  from  the  buffer  and  sends  it  to 
the  terminal,  which  then  generates  another  interrupt.  This  operation  continues  until  the 
buffer  is  empty  and  the  control  process  terminates  the  interrupt  cycle. 
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V.  YAMABICO  SOFTWARE  DEVELOPMENT  ENVIRONMENT 


The  current  software  development  environment  for  the  robot  is  restricted  to  a  single 
Sun-3  workstation  using  SunOS  4.1.1.  The  reason  is  that  this  workstation  shares  the  same 
processor  architecture  as  the  robot.  The  development  cycle  will  improve  when  the  robot’s 
CPU  system  is  upgraded  to  a  SPARC-4  processor  due  to  more  advanced  development 
tools,  FTP  transfer  capability  and  support  for  system  I/O  by  the  resident  debugger. 

The  Yamabico-11  does  not  use  a  commercial  operating  system.  This  means  the  user 
must  perform  some  of  the  basic  operating  system  functions  through  the  resident  debugger. 
First,  the  user  must  load  the  program  into  memory.  Then  control  of  the  CPU  must  be 
transferred  from  the  debugger  program  to  the  user’s  program  by  placing  the  address  of  the 
first  instruction  to  be  executed  into  the  program  counter  (pc).  The  user  must  also  ensure  the 
program  returns  control  to  the  debugger  when  it  has  terminated.  Preparations  for  these 
functions  begin  with  the  compilation  phase. 

A.  C  COMPILERS 

The  only  compilers  available  for  the  current  development  environment  are  the  Unix  C 
Compiler  and  the  GNU  Project  C  Compiler.  Both  compilers  are  invoked  through  a 
compiler  driver. 

1.  Compiler  Drivers 

As  shown  in  Figure  18,  a  compiler  driver  is  a  program  that  creates  executable 
code  by  sequentially  calling  the  pre-processor,  compiler,  assembler  and  linker  with  the 
appropriately  supplied  parameters  and  files.  The  driver  for  the  Unix  C  compiler  is  cc,  while 
the  driver  for  the  GNU  C  compiler  is  gcc.  When  invoked,  the  driver  passes  the  C  source 
file  to  the  pre-processor.  Output  from  the  pre-processor  is  then  passed  to  the  compiler.  Each 
compiler  is  essentially  a  translator,  translating  the  C  source  file  into  equivalent  assembly 
code.  The  compiler  driver  then  passes  this  assembly  code  to  the  assembler  to  create  object 
code.  Next  the  driver  passes  the  assembler  output  to  the  linker  to  build  executable  code.  The 
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Figure  18:  Compiler  Driver  Process 

output  from  the  linker  is  then  stored  in  a  file  in  the  developer’s  directory.  This  process  can 
be  verified  by  using  compiler  flags  to  display  the  compiler  driver  commands.  These  flags 
are  -dryrun  for  cc  and  -v  for  gcc  [CPG  88][Stallman  89]. 

2.  Code  Optimization 

Producing  optimized  object  code  is  one  of  the  goals  of  program  development. 
Both  compilers  will  optimize  source  code  if  the  -O  flag  is  used.  However,  this  is  not  always 
desirable.  There  are  instances  where  several  values  must  be  assigned  to  a  variable  or 
address  in  succession,  as  in  initializing  serial  ports  for  example.  If  this  optimization 
function  is  enabled,  the  compiler  would  eliminate  all  but  the  last  assignment,  resulting  in 
an  improperly  initialized  port.  This  can  be  verified  by  inspecting  the  assembly  code 
translation  produced  when  the  flag  -S  is  used  with  -O.  The  -S  flag  instructs  the  compiler 
driver  to  stop  after  translating  the  C  source  file  into  assembly  code,  storing  the  translation 
in  a  file.  This  file  will  be  given  the  same  name  as  the  source  code  with  the  exception  of  a  .s 
extension.  Inspection  of  the  assembly  code  will  reveal  that  only  the  last  assignment  is 
retained.  Therefore  the  developer  must  be  careful  in  deciding  which  modules  can  be  safely 
optimized  by  the  compiler  [CPG  88][Stallman  89]. 

Another  way  of  optimizing  code  is  by  using  ‘#define’  statements  to  declare 
constants  instead  of  ‘const.’  Constants  declared  using  ‘const’  are  stored  on  the  stack  during 
run  time.  References  are  implicit,  using  the  stack  pointer  and  an  offset.  Using  the  ‘#define’ 
method  causes  the  compiler’s  pre-processor  to  substitute  the  actual  value  into  the  source 
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code  prior  to  compilation.  This  is  the  equivalent  to  hard  coding  the  values  except  that  it  is 
cleaner  and  easier  to  maintain/modify. 

3.  Standard  Libraries 

It  is  assumed  that  the  generated  executable  code  will  be  run  on  the  platform  used 
for  development  Therefore  each  compiler  uses  and  depends  on  its  own  set  of  standard 
libraries.  These  libraries  are  usually  linked  with  the  object  code  automatically  by  the 
compiler  driver  when  creating  the  executable.  However,  some  of  the  routines  in  these 
libraries  make  calls  to  the  operating  system.  Since  the  robot  does  not  have  an  operating 
system,  these  libraries  should  not  be  used.  Standard  C  functions  for  input/output  or  file 
operations  such  as  printf( )  should  also  not  be  used.  Therefore,  the  compiler  driver  should 
be  instructed  to  stop  before  the  linking  phase  by  using  the  -c  flag.  It  will  then  save  an  object 
file  with  a  .o  extension  for  each  C  source  file  passed  to  the  compiler  driver.  These  files  can 
then  be  used  in  an  explicit  call  to  linker  without  the  libraries  [CPG  88][Stallman  89], 

4.  CC  vs.  GCC 

There  is  a  major  difference  between  the  two  compilers.  The  Unix  compiler  is  for 
source  code  developed  using  the  K&R  standard,  a  style  of  developing  C  source  code 
designed  by  Brian  Kemighan  and  Dennis  Ritchie.  This  standard  was  developed  prior  to  the 
ANSI  standard  adopted  in  1989.  The  GNU  compiler  was  designed  for  source  code  written 
in  ANSI  C.  The  use  of  function  prototypes  for  parameter  checking  is  one  of  the  major 
advantages  the  ANSI  standard  has  over  the  K&R  standard. 

5.  Using  GCC 

The  GNU  Project  C  Compiler  does  have  some  peculiarities  that  must  be  taken 
into  consideration  when  developing  programs  for  the  robot.  First,  the  compiler  generates  a 

call  to _ main()  as  the  first  statement  in  main( ).  Since  gee's  libraries  are  not  linked  with  the 

object  code,  the  result  will  be  an  undefined  symbol  error  during  linking.  Therefore,  the 
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developer  must  explicitly  create  a  function  called _ main( )  that  returns  void.  The  function 

definition  should  only  include  a  simple  return  statement. 

The  compiler  may  also  insert  calls  to  certain  standard  library  routines,  such  as 
memcpy( )  and  memsetf ).  Since  the  libraries  are  not  included  during  the  linking  phase,  again 
the  result  will  be  an  undefined  symbol  error.  Therefore  the  user  must  either  develop  these 
functions  or  build  a  new  library  for  the  robot.  The  Unix  ar  command  can  be  used  to  extract 
copies  of  these  functions  from  the  standard  libraries  and  to  build  the  new  library  [SRM  88]. 

Another  precaution  is  associated  with  how  the  compiler  generates  storage  for 
locally  declared  strings.  Storage  for  strings  declared  within  a  function  body  is  allocated 
immediately  prior  to  the  address  of  the  function  itself.  Currently,  this  only  effects  the 
functions  main()  and  user()  since  they  must  start  at  an  absolute  address.  This  is  discussed 
later  in  this  chapter.  Therefore,  the  developer  should  not  declare  local  strings  in  either 
function. 

Two  flags  that  are  helpful  to  use  are  -Wall  and  -Wpointer-arith.  The  -Wall  flag 
instructs  the  compiler  to  issue  common  warning  messages.  For  example  messages  will  be 
displayed  when  variables  are  declared  but  not  used,  when  functions  are  implicitly  declared 
as  returning  an  integer  or  when  functions  declared  to  return  a  non-void  value  but  do  not 
have  a  return  statement  in  the  function  definintion.  To  receive  warnings  concerning  the  use 
of  pointer  arithmatic,  use  the  -Wpointer-arith  flag.  This  allows  the  developer  to  find 
accidental  uses  of  pointer  arithmatic  [Stallman  89]. 

B.  LINKING 

Each  object  file  passed  to  the  Unix  linker  (Id)  consists  of  a  text  segment  and  a  symbol 
table.  The  text  segment  contains  the  executable  instructions  while  the  symbol  table  contains 
the  symbols  (function  names  and  global  variables)  that  can  be  accessed  by  other  modules. 
When  multiple  object  Ties  are  used,  the  linker  starts  by  appending  the  text  segment  of  the 
second  file  to  the  first’s.  The  text  segment  of  each  additional  object  file  is  then  appended 
in  the  order  given  to  create  one  text  segment.  All  symbols  are  also  collected  into  one 
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symbol  table  [SRM  88].  Figure  19  shows  an  example  object  module  created  from  the 
following  command:  Id  main.o  iosys.o  serial.o. 


main.o  functions 


iosys.o  functions 


serial.o  functions 


symbol  table:  includes 
global  variable  names 
and  function  names  of 
main.o.  iosys.o  and 
serial.o 


Figure  19:  Example  of  Composite  Object  File 


After  all  object  files  have  been  processed,  the  linker  must  calculate  the  absolute 
address  of  each  symbol  in  the  symbol  table.  Addresses  for  functions  are  determined  using 
the  offset  relative  to  the  beginning  of  the  program  and  the  program’s  starting  location. 
Absolute  address  calculation  for  global  variables  is  based  on  an  offset  from  the  next  page 
boundary  following  the  text  segment.  It  then  resolves  all  references  to  these  symbols. 

After  all  references  are  resolved,  the  linker  creates  a  load  module.  This  module  is  a  file 
of  records  used  by  the  loader.  These  records  contain  either  instructions  or  initialized  global 
data  along  with  the  address  in  memory  where  they  are  to  be  located  by  the  loader.  All  of 
the  records  containing  text  (instructions)  are  collected  and  placed  prior  to  the  records 
containing  the  initialized  global  data. 
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1.  The  Kernel  Module 


Currently,  all  programs  are  divided  into  two  load  modules.  One  module  is  the 
kernel  while  the  other  contains  the  motion  commands.  The  kernel  contains  main( )  and  most 
of  the  code.  As  mentioned  earlier,  the  user  must  know  where  the  program  execution  begins. 
It  is  standard  practice  to  load  the  kernel  at  hexadecimal  address  0x304000.  Therefore,  the 
first  instruction  in  the  function  main()  must  be  located  at  this  address.  To  correctly  resolve 
the  symbolic  references,  the  kernel's  starting  location  must  be  passed  to  the  linker  by  using 
the  -T  304000  flag  [SRM  83]. 

Since  the  creation  of  the  text  segment  is  based  on  the  order  of  object  files  passed 
to  the  linker,  the  object  file  containing  the  definition  of  main( )  must  be  the  first  file  in  that 
list.  The  order  of  the  remaining  files  is  unimportant.  Additionally,  since  cc  and  gcc  generate 
object  code  as  defined  in  the  C  source  file,  main( )  must  be  the  first  function  defined  in  its 
source  file.  Finally,  if  local  strings  are  defined  in  main( ),  gcc  will  reserve  memory  storage 
prior  to  main().  Consequently,  0x304000  would  be  the  address  of  the  string  instead  of  the 
first  executable  instruction.  These  last  two  points  can  be  verified  by  using  the  -S  flag  and 
inspecting  the  resulting  assembly  code.  For  an  example,  see  Figure  20. 

C  Source  Assembly  Translation 


Figure  20:  Effects  of  Compilation  on  Strings 
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2.  The  User  Module 


The  user  module  contains  MML  library  function  calls.  Its  construction  is  similar 
to  the  kernel  with  a  few  exceptions.  First,  the  function  User()  is  the  primary  function 
instead  of  main().  Second,  the  module  is  loaded  at  hexadecimal  address  0x334000  vice 
0x304000. 

Since  User()  is  called  by  the  kernel ,  the  kernel  must  know  User()' s  location. 
However,  User()  must  also  know  the  location  of  the  functions  in  the  kernel  too.  Making 
User( )  accessible  to  the  kernel  is  accomplished  by  declaring  pointer  to  a  function  returning 
void  within  the  kernel,  and  initializing  it  to  0x334000.  To  provide  access  to  the  kernel's 
functions,  use  the  -A  kernel  flag  to  link  the  user  module  with  kernel's  symbol  table.  One 
note  of  caution,  the  user  module  is  now  dependent  on  the  kernel.  Therefore  any  change  to 
the  kernel  requires  the  user  module  to  be  re-linked. 

C.  LOADING 

Before  a  program  can  be  executed,  it  must  be  placed  into  random  access  memory  by  a 
loader.  Each  record  in  the  load  module  contains  instructions  or  initialized  data  information 
along  with  an  address.  The  loader  then  loads  the  information  from  each  record  at  the 
address  specified. 

However,  the  loader  for  the  robot  is  a  simple  program  that  essentially  dumps  the 
information  sequentially  from  all  records  beginning  at  either  0x304000  or  0x334000.  This 
does  not  interfere  with  the  placement  of  the  program  instructions.  But  the  global  variable 
initialization  data  is  erroneously  placed  at  the  end  of  the  text  segment  instead  of  in  the 
global  variables.  Therefore,  global  variables  initialized  at  compile  time  actually  contain 
unpredictable  data  when  the  program  is  loaded.  The  solution  to  this  problem  is  to  initialize 
all  data  at  run-time,  prior  to  executing  the  program.  Figure  21  shows  the  robot’s  memory 
framework  after  program  loading. 
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Figure  21:  Yamabico-ll  Memory  Map 

D.  DEBUGGING  TOOLS 

1.  The  Onboard  Debugger 

As  mentioned  earlier,  the  CPU  system  includes  the  VME7920  Debugging 
Package.  This  monitor  program  is  primarily  used  to  load  and  execute  programs  on  the 
robot.  However,  it  can  be  used  to  debug  programs.  The  developer  can  set  breakpoints  at  a 
particular  instruction,  halting  program  execution.  The  value  of  variables  or  registers  can  be 
examined  and  modified.  The  debugger  will  even  aid  the  developer  by  displaying  machine 
code  in  assembly  instruction  format  The  only  requirement  is  knowledge  of  the 
instruction’s  or  variable’s  memory  location  [VDP  86]. 
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2.  The  Unix  nm  Command 

To  obtain  the  address  of  variables  or  functions  from  an  executable  file,  the  Unix 
nm  command  can  be  used.  The  nm  command  displays  a  files  symbol  table.  For  example, 
entering  nm  -n  user  will  generate  a  listing  containing  each  symbol  (global  variable  or 
function  name)  in  user's  symbol  table  along  with  its  location  in  memory.  The  -n  flag 
instructs  the  command  to  sort  the  list  by  memory  location.  This  listing  gives  the  developer 
the  memory  address  locations  needed  to  debug  programs  with  the  VME7920  Debugging 
Package  [SRM  88]. 
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VI.  SYSTEM  IMPLEMENTATION  AND  TESTING 


ANSI  C  was  the  language  chosen  to  implement  the  new  design  instead  of  the  older 
K&R  version  used  in  MML- 10  since  it  provides  stronger  type  checking  through  prototypes. 
Since  the  two  versions  are  similar,  some  of  MML-lO’s  source  code  could  be  ported  to  the 
new  design  with  minor  modifications.  ANSI  C  also  enables  a  smooth  transition  of  MML  to 
an  object  oriented  paradigm  using  C++.  However,  the  only  ANSI  C  compiler  locally 
available  for  the  robot’s  software  development  environment  is  GNU’s  gcc. 

A.  IMPLEMENTATION  TECHNIQUES 

1.  Stability 

Producing  a  stable  system  is  one  of  the  primary  objectives  during  the 
implementation  phase.  One  method  for  producing  a  stable  system  is  to  use  encapsulation. 
In  C,  this  is  accomplished  by  declaring  global  variables  and  local  functions  with  the  static 
specifier.  These  identifiers  can  be  referenced  from  sources  within  the  file  but  prevents 
external  references. 

Reducing  unnecessary  pointers  and  pointer  arithmetic  is  another  way  of 
producing  a  stable  system.  Generally,  there  are  only  two  reasons  to  call  a  function.  The  first 
reason  is  to  produce  some  action,  such  as  displaying  a  value.  In  this  case,  actual  values  are 
passed  as  parameters  and  the  function  does  not  return  a  value.  The  second  reason  to  call  a 
function  is  to  process  information,  producing  one  or  more  results.  In  this  case,  it  is  common 
to  let  the  function  assign  the  values  directly  by  passing  the  addresses  of  these  variables  as 
parameters.  However,  this  can  lead  to  many  programming  errors.  A  better  method  for 
writing  these  functions  is  to  use  a  return  statement.  To  return  more  than  a  single  value,  a 
structure  of  values  should  be  used. 


38 


2.  Portability 


cl  Required  Assembly  Code 

Each  assembly  instruction  corresponds  to  a  single  machine  instruction.  It 
also  produces  hardware  dependencies  since  each  processor  has  its  own  assembly  language. 
Therefore  programming  in  assembly  language  is  very  difficult  and  should  be  avoided. 

One  reason  to  use  assembly  language  programming  is  to  increase  the  speed 
of  program  execution  [Schildt  90].  Although  MML  is  a  real-time  system  with  timing 
constraints,  it  is  still  a  research  project.  Using  assembly  language  to  increase  program 
execution  simply  adds  complexity. 

The  primary  reason  to  use  assembly  language  is  to  access  specific  hardware 
components  or  instructions  that  can  not  be  accessed  in  any  other  manner  [Schildt  90].  The 
status  register  and  rte  instruction  are  examples  of  such  requirements  when  using  the 
Motorola  68020  processor.  Modification  of  the  status  register  is  handled  by  instructions 
used  for  special  registers.  The  rte  is  used  to  return  from  an  interrupt  handling  routine 
instead  of  the  normal  rts  instruction. 

b.  Handling  Interrupts 

Setting  up  an  interrupt  mechanism  requires  two  steps.  First,  an  interrupt 
handler  must  be  created.  This  routine  is  a  basic  shell  that  must  be  written  in  assembly 
language.  The  routine  must  begin  by  saving  all  of  the  CPU’s  registers  onto  the  stack.  If  a 
coprocessor  is  used,  those  registers  must  be  saved  as  well.  Then  the  routine  must  make  a 
call  to  a  C  function  that  will  control  the  real  processing  during  the  interrupt  cycle.  Once 
control  is  returned  from  the  C  function,  the  interrupt  handler  must  restore  each  register  to 
the  value  it  contained  prior  to  the  interrupt.  It  is  important  to  note  that  the  registers  must  be 
restored  in  the  reverse  order  as  they  were  saved.  Finally,  the  rte  instruction  is  used  to  return 
from  the  interrupt 

The  second  step  in  establishing  an  interrupt  mechanism  is  to  setup  the  CPU 
and  interrupting  hardware  component.  This  is  a  two  step  process.  First,  the  address  of  the 
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interrupt  handler  must  be  placed  in  the  CPU’s  vector  table.  Next,  the  index  value  of  the 
table  entry  is  passed  to  the  interrupting  device.  Once  these  two  steps  are  complete,  the 
hardware  device  can  be  enabled. 

c.  Absolute  Addressing  in  C 

In  C,  each  variable  declared  as  a  pointer  stores  a  32-bi*  memory  address  and 

can  be  assigned  constant  values  between  zero  and  232-l.  The  amount  of  memory  that  a 
pointer  references  depends  on  the  pointer’s  declaration.  For  example,  if  a  variable  is 
declared  as  a  character  pointer,  only  a  single  byte,  located  at  pointer’s  value,  will  be 
referenced.  But,  if  the  variable  is  declared  as  an  integer  pointer,  then  four  bytes  will  be 
referenced  by  the  pointer,  starting  at  the  value  of  the  pointer. 

The  amount  of  memory  that  a  pointer  references  can  be  temporarily 
changed.  Using  the  cast  operator,  a  pointer  is  changed  to  another  pointer  type.  However, 
the  change  only  lasts  for  that  operation.  Afterwards  the  pointer  reverts  back  to  a  pointer  of 
its  declared  form. 

3.  Readability 

One  technique  for  making  a  system  more  readable  is  to  use  descriptive  names 
for  functions,  variables  and  constants.  Abbreviations  should  never  be  used.  When  declaring 
a  symbol,  two  conventions  are  generally  used:  separate  multiple  words  with  underscores  or 
capitalize  the  first  letter  of  each  word  in  the  symbol.  The  later  convention  is  used 
throughout  this  implementation.  Another  naming  convention  utilized  in  this 
implementation  is  to  declare  or  define  constant  symbols  with  all  capital  letters. 

Header  files  are  used  to  declare  the  prototypes  for  each  function  in  a  module  that 
can  be  accessed  using  external  calls.  This  presents  the  file’s  interface  and  should  be  well 
documented.  The  comments  should  describe  what  parameters  are  required  and  the  expected 
results.  A  description  of  the  algorithm  is  not  needed.  However,  constants  needed  by  a  C  file 
that  may  also  be  referenced  by  other  files  are  included  in  its  header  file  also. 
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Another  technique  to  making  a  system  more  readable  is  to  limit  the  size  of  the 
functions  to  one  page.  Techniques  such  as  page  breaks  between  functions  and  good 
descriptive  comments  should  also  be  used. 

4.  Backward  Compatibility 

To  maintain  backward  compatibility,  the  new  system  generates  user  and  kernel 
object  files.  The  kernel  is  still  loaded  at  0x304000  and  user  is  loaded  at  0x334000.  The 
absolute  address  of  user()  is  declared  in  motorola.asm.s  as  _user.  To  prevent  the  linking 
error  of  duplicate  symbols,  the  actual  name  of  the  user  function  in  user.c  should  be  User() 
instead  of  user().  However,  to  transfer  control  to  the  user  program,  main()  is  still  required 
to  call  user()  instead  of  User(). 

To  maintain  compatibility  with  MML-lO’s  immediate  command  names,  a 
translation  header  file  was  created.  This  is  a  file  containing  macros,  used  by  a  pre¬ 
processor,  that  replace  the  old  command  syntax  with  the  new  command  syntax.  A  similar 
one  should  be  created  for  other  MMH0  commands  so  that  previously  created  user 
programs  can  operate  with  the  new  system. 

B.  SYSTEM  IMPLEMENTATION 

Due  to  the  scope  of  the  design,  it  was  decided  that  only  a  core  portion  of  the  new  design 
would  be  implemented,  starting  with  the  CPU  system.  The  system  described  in  this  section 
consists  of  files  with  .c,  .h,  and  J  extensions.  Those  files  ending  with  a  .c  are  C  source  files. 
These  files  contain  function  definitions  and  encapsulated  data  structures.  The  .h  files  are 
the  header  files,  while  the  files  ending  with  a  .s  are  written  in  assembly  language.  With  the 
exception  of  main.c,  each  C  and  assembly  file  has  an  associated  header  file.  All  files  are 
presented  in  the  Appendices  and  were  developed  as  part  of  this  study  except  for  a  few 
routines  found  wheels.c  and  motorola.asm.s.  These  routines  were  ported  from  MML-10. 
However,  the  system  does  make  use  of  some  utility  functions,  such  as  memory 
management  and  math  routines  that  were  developed  in  earlier  projects. 
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1.  The  CPU  System 


The  CPU  system  was  implemented  first  since  it  is  required  to  initialize  all  other 
systems.  Appendix  A  contains  the  source  code  for  this  system.  It  contains  the  files: 
definitions. h,  main.c,  system.h,  system.c,  motorola.h,  motorola.asm.s,  se  dal.h,  and  serial.c. 

The  definitions. h  file  contains  the  system’s  standard  type  declarations.  Some  of 
the  new  data  types  refer  to  constructs  used  by  the  motion  system.  Other  declarations  define 
standard  assembly  language  data  types,  such  as  BYTE  and  WORD.  These  definitions  make 
the  code  easier  to  follow. 

The  file  main.c  only  contains  mainv)  to  prevent  the  linking  problem  discussed 
earlier.  This  is  the  function  that  gets  loaded  at  0x304000.  It  initializes  tl;~  sub-systems  and 
then  calls  user(),  the  user  program.  After  the  user  program  is  finished,  rexit()  is  called  to 
return  control  back  to  the  resident  debugger. 

The  Initialization  routines  called  by  mainQ  are  located  in  system.c.  They  are  used 
to  setup  the  interrupt  mechanisms  described  earlier.  Also  included  in  this  file  are  the  some 

system  functions  for  enabling  and  disabling  interrupts  and  the _ main()  function  required 

by  the  gcc  compiler. 

The  motorola.asm.s  module  contains  all  of  MML’s  required  assembly  code.  The 
interrupt  handling  shells  described  above  are  located  in  this  module.  Also  defined  here  is  a 
routine  to  change  the  CPU’s  interrupt  priority  level.  The  rexit()  routine  is  located  in  this 
module  since  it  uses  a  sequence  of  instructions  that  are  difficult  to  duplicate  using  a  high 
level  language.  The  address  of  user()  is  also  set  in  this  file. 

The  serial.c  module  contains  the  serial  system.  As  mentioned  earlier,  this  is  a 
component  of  the  CPU  system.  It  serves  as  an  interface  between  the  serial  boards  and  the 
other  sub-systems.  The  previously  discussed  technique  for  referencing  absolute  memory 
locations  is  evident  in  this  module. 
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2.  The  Terminal  System 

The  terminal  system  was  chosen  as  the  next  system  to  implement  as  this  would 
aid  in  debugging  other  systems.  The  files  iosys.h  and  iosys.c  contain  the  system’s  source 
code  and  are  located  in  Appendix  B.  The  functions  defined  in  iosys.c  provide  input  and 
output  for  string,  integer  and  real  number  data  types. 

Some  of  the  functions  depend  on  conversion  routines.  Some  routines  convert 
strings  to  integer  or  real  values.  Others  convert  integer  and  real  values  to  strings.  Notice 
how  the  global  data  is  encapsulated  by  using  the  static  specifier.  This  prevents  routines 
outside  this  module  from  accessing  these  variables. 

3.  The  Motion  Control  System 

Appendix  C  contains  the  files:  motion.h,  motion.c,  motiontrace.h  motiontrace.c, 
wheels.h,  and  wheels. c.  These  files  define  the  motion  control  system,  motion.c  defines  the 
functions  that  support  odometry  and  velocity  control.  The  wheel  system  component  is 
defined  m  the  wheels.c  module  while  motiontrace.c  defines  motion’s  tracing  system 
component  The  tracing  system  depends  on  memory  management  routines.  The  motion 
control  rules  required  for  path  following  are  not  implemented  in  this  core  system. 

4.  The  User  Program 

A  sample  user.c  is  included  in  Appendix  D  along  with  its  user.h.  This  module 
shows  how  to  use  the  core  system.  Another  file  associated  with  the  user  system  is 
compatability.h.  This  file  provides  the  translation  layer  for  older  user  programs  developed 
for  MML-10. 

C.  SYSTEM  TESTING 

Since  the  system  was  incrementally  developed,  testing  was  conducted  at  each  system 
modification.  This  section  describes  the  testing  techniques  that  proved  most  helpful. 
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1.  Using  an  External  Power  Supply 

The  robot’s  battery  power  was  often  drained  due  to  software  testing  by  several 
people.  System  testing  was  aided  by  an  AC  power  supply.  To  test  the  motion  control 
system,  the  robot  was  placed  on  wooden  blocks  to  prevent  movement  The  power  supply 
provided  sufficient  power  when  testing  the  CPU  and  terminal  systems.  However,  it  was 
discovered  that  the  generator  did  not  produce  enough  power  to  drive  the  wheels  above 
moderate  speeds.  Fast  wheel  movements  caused  the  CPU  to  shut  down  due  to  insufficient 
power.  So  stationary  testing  was  conducted  at  slow  speeds. 

2.  Output  from  Interrupt  Handlers 

Displaying  information  to  the  terminal  from  the  motion  control  system  required 
special  handling.  The  problem  was  that  the  information  would  be  sent  to  the  terminal 
system  every  10  milliseconds,  causing  the  output  buffer  to  overflow.  The  solution  was  to 
send  the  messages  at  a  lower  frequency.  This  was  accomplished  by  using  the  LoopTest 
variable  in  motion.c  as  a  counter.  Every  motion  control  cycle  incremented  the  LoopTest 
When  the  counter  reached  the  frequency  value  of  100,  it  would  be  reset  to  zero.  All  output 
messages  were  then  placed  in  a  conditional  block  that  checked  the  value  of  LoopTest.  Only 
when  it  was  zero  would  the  message  be  sent  to  the  terminal  system.  The  result  is  that  the 
message  would  be  sent  to  the  terminal  every  second  instead  of  every  10  milliseconds. 

3.  Measurements 

The  first  part  of  the  motion  control  system  to  be  implemented  was  the  wheel 
system  component.  Manual  calculations  were  required  to  verify  the  operation  of  this 
system.  To  verify  the  change  in  distance  and  orientation  information,  the  robot  was 
manually  pushed  along  a  path  with  known  distance.  Several  trials  were  performed  using 
paths  of  different  lengths.  When  the  robot’s  results  were  compared  to  the  path  lengths 
measured  with  a  measuring  tape,  the  differences  were  consistently  within  one  centimeter. 

To  verify  the  velocity  inputs  to  the  wheel  system,  the  tracing  component  is  used. 
Figure  22  is  an  sample  of  results  obtained  by  changing  the  robot’s  commanded  velocity 
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Figure  22:  Velocity  Control  Results 


from  zero  to  20  cm/s.  The  results  show  that  the  actual  velocity  is  one  to  two  centimeters  per 
second  faster  than  the  commanded  velocity.  This  difference  was  found  at  all  speeds  tested 
and  is  the  result  of  a  lookup  table  used  to  calculate  pwm  values.  This  table  is  tailored  for 
this  specific  system  and  requires  further  testing  to  correct  the  discrepancies. 
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VII.  CONCLUSIONS 


A.  RESULTS 

The  end  product  is  a  solid  core  system,  capable  of  serving  as  a  foundation  for  further 
MML  research.  The  system  is  implemented  using  a  standardized  language.  Hardware 
dependencies  are  localized  and  the  required  assembly  code  is  reduced  to  a  single  module. 
This  results  in  a  system  that  is  very  portable.  Each  module  is  provided  with  a  clean  interface 
making  the  system  very  modular.  With  limited  pointer  references  and  data  encapsulation 
the  system  is  also  very  stable.  The  result  is  a  system  that  is  very  readable,  easy  to  maintain 
and  easy  to  modify. 

One  of  the  underlying  themes  presented  in  this  study  is  pointer  reduction.  This  is  not 
to  imply  an  unfamiliarity  with  the  C  language  or  the  use  of  pointers.  In  fact  the  author  is 
aware  of  the  value  of  using  pointers  and  has  been  programming  with  pointers  in  the  C 
language  since  1985.  However,  many  of  the  researchers  on  the  MML  project  do  not  possess 
the  same  experience  level.  This  has  resulted  in  improper  pointer  usage,  thereby  increasing 
program  complexity  and  decreasing  stability.  This  is  the  primary  reason  for  recommending 
the  reduction  pointer  usage. 

B.  RECOMMENDATIONS 

The  next  step  for  this  new  system  is  to  complete  the  motion  control  system.  This 
requires  the  implementing  of  the  motion  control  rules  to  provide  path  following.  It  also 
requires  the  addition  of  the  sequential  commands  and  the  instruction  queue. 

Following  the  completion  of  the  motion  control  system,  the  sonar  system  should  be 
implemented.  This  will  involve  writing  the  interrupt  handler  shell  and  control  process.  The 
assembly  code  should  also  be  converted  to  ensure  portability. 

Another  potential  project  is  to  develop  a  simulator  based  on  the  new  system.  Due  to 
the  system’s  modularity,  this  should  only  require  the  simulation  of  the  interrupt 
mechanisms  and  the  hardware  dependent  code. 
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APPENDIX  A 


A.  DEFINITIONS.H 

Author(s):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Da.a:  Decembers,  1993 

Revised:  March  4, 1994 

File  Name:  definitions.h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  standard  definitions  and  data  type 

declarations  used  throughout  the  reset  of  the  MML  system. 


#ifndef  _DEFINITIONS_H 
#define  _DEFINITIONS_H 


#define  MOTION_CONTROL_CYCLE  0.01 

#define  M*X_REAL_PRECISION  15 
#define  MAX_iNTEGER_DIGITS  19 

typedef  enum  {FALSE  «  0,  TRUE}  BOOLEAN; 

typedef  unsigned  char  BYTE; 
typedef  unsigned  short  WORD; 
typedef  unsigned  long  LONG; 
typedef  unsigned  long*  ADDRESS; 


typedef  struct{ 
double  Linear; 
double  Rotational; 
}  VELOCITY; 


typedef  struct} 
double  XPosition; 
double  YPosition; 
}  POINT; 


typedef  struct! 
POINT  Position; 
double  Orientation; 
double  Kappa; 
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}  CONFIGURATION; 


#endif 


B.  MAIN.C 


Auihor(s):  Scott  Book 

Project-  Yar.iabico  Robot  Control  System 

Date:  Decembers,  1993 

Reviseo:  March  4, 1 994 

File  Name:  main.c 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  main().  Its  purpose  is  to  initialize  all 
sub-systems  and  then  pass  control  to  user().  Once  user()  is 
complete,  the  routine  returns  control  to  the  resident  debugger. 
. . ***** . . . . . ***** . / 


#include  “definitions.h" 
#include  “motorola.h” 
#include  “system.h” 
#include  “memsys.h” 
#include  “iosys.h” 
#include  “serial.h" 
#include  “motion. h” 
#include  “user.h” 

void  user(); 

void  main(){ 

CpuSyslnitialize(); 

ResetSerialBoards(); 

Set  I  nterruptPrio  rity{7) ; 

InitializeMemSysO; 

lnitializeConsole(); 

lnitializeHost(); 

loSyslnitialize(); 

MotionSyslnitialize(); 

SetlnterruptPriority(O) ; 

user{); 
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rexit() ; 

} 


C.  SYSTEM.H 

r . . . . . . . . 

Author(s):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date:  Decembers,  1993 

Revised:  March  4, 1994 

File  Name:  system.h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  prototypes  for  routines  that  are  hardware 
dependent. 

. * . ****** . * . / 

#ifndef _ SYSTEM_H 

#define  SYSTEM  H 


r 


Function _ main()  is  an  empty  function.  It  is  required  if  gcc  is  used  since 

gcc  will  insert  a  call  to _ main{)  within  mainQ. 

. . * . * . * . * . * . * . ***‘7 


void _ main(); 


r . * . * . 

Function  CpuSysInitializef)  is  used  to  set  up  any  requirements  that  are 
specific  to  the  Motorola  68020  CPU. 


void  CpuSyslnitialize(); 


r . * . . . 

Function  Enablelnterrupts()  turns  on  interrupt  servicing. 


void  EnablelnterruptsQ; 
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r . ******* . . . 

Function  Disablelnterrupts()  turns  off  interrupt  servicing. 


void  Disablelnterrupts(); 


/ 


. .............................. 

Function  SetConsolelntMechanism()  establishes  the  console’s  interrupt 
driven  output  mechanism. 

....................................... . . / 


void  SetConsolelntMechanism(); 
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r . ** . * . 

Function  SetMotionlntMechanism()  establishes  the  synchronous  interrupt 
mechanism  required  for  motion  control. 


void  SetMotionlntMechanism(); 


#endif 


D.  SYSTEM.C 

/********************************************************“************“******** 
Author(s):  Scott  Book 
Project:  Yamabico  Robot  Control  System 
Date:  Decembers,  1993 

Revised:  March  4, 1994 
File  Name:  system.c 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  required  cpu  specific  routines.  The  file 
is  seperated  into  two  sections:  private  and  public.  The  private 
section  contains  the  encapsulated  data  while  the  public  section 
contains  the  routines  available  to  other  systems/modules. 

*******************************************************************************7 


#include  “definitions.h” 
#include  “motorola.h” 
#include  “serial.h" 
#include  “system.h” 


static  int  Saved  Interrupt  Mask; 
static  BOOLEAN  InterruptsOn; 


r******************************************** . . 

Routine _ main  is  required  when  using  the  ‘gcc'  compiler.  This  is  because  the 

compiler  inserts  a  call  to  this  routine  at  the  beginning  of  the  main  function 
defined  for  the  program.  This  is  normally  taken  care  of  by  linking  in  the 
bootstrap  object  modules,  however  these  are  not  added  to  a  program  that 
operates  without  an  operating  system  such  as  the  mml  program.  Therefore,  since 
this  routine  is  called,  the  only  requirement  is  for  this  routine  to  simply 
return  back  to  the  main  program. 

* - - - - - - — . — ***** — ****** - / 


void _ main(){ 

return; 

} 
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I********************** >**••*•*•*«**•*•**•••******•*##*******»•••**•«*•*******#* 

Function  CpuSyslnitialize()  is  used  to  set  up  any  requirements  that  are 
specific  to  the  Motorola  68020  CPU. 


void  CpuSyslnitialize{){ 

ADDRESS  VectorTableBaseAddress  *  GetVectorBase(); 

r********************************************************************** 

*  Use  a  blank  interrupt  handler  to  recover  from  unexpected  exceptions 

*  such  as  when  the  status  word  is  set  to  a  specific  value  in  function 

*  SetlnterruptPriority.  So  enter  the  address  of  that  interrupt  handling 

*  routine  into  the  255th  entry  of  the  exception/intenupt  vector  table. 

............................ — . ***** . / 


'(VectorTableBaseAddress  +  255)  -  (LONG)Unexpected; 

Interrupts  On  -  TRUE; 

SavedlnterruptMask  =  0; 

} 


/****** . ................................ . . 

Function  Enablelnterrupts()  allows  other  systems  to  reset  the  interrupt  mask 
to  the  previously  stored  value. 

. * . . . ****** . */ 


void  Enablelnterrupts(){ 
if(lnterruptsOn  «  FALSE){ 

InterruptsOn  «  TRUE; 
SetlnterruptPriority(SavedlnterruptMask); 

} 

} 


Function  Disablelnterrupts()  allows  other  systems  disable  all  interrupt 
servicing.  The  current  interrupt  mask  is  saved  so  that  it  can  be  restored 
later. 

- .... - - - - - - - .., 


void  Disab!elnterrupts(){ 
if(lnterruptsOn  «  TRUE){ 

SavedlnterruptMask  -  SetlnterruptPriority(7); 
InterruptsOn  -  FALSE; 

) 

} 
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/ — * - * - * — * - ****** - * - 

Function  SetConsolelntMechanism()  establishes  the  console’s  interrupt 
driven  output  mechanism. 


void  SetConsolelntMechanism(){ 

ADDRESS  VectorTableBaseAddress  =  GetVectorBase(); 

DisablelnterruptsO; 

/*  Enter  the  address  of  the  interrupt  handling  routine  into  the  65th  */ 
r  entry  of  the  exception/interrupt  vector  table  of  the  CPU.  */ 
‘(VectorTableBaseAddress  +  65)  =  (LONG)loSysHandler; 

SetLatch(0xffff0031 ,65); 

Enablelnterrupts(); 

} 


/*•****************************************************************************** 
Function  SetMotionlntMechanism()  establishes  the  synchronous  interrupt 
mechanism  required  for  motion  control. 

. . . . . . . . . / 


void  SetMotionlntMechanism{){ 

ADDRESS  VectorTableBaseAddress  =  GetVectorBase(); 

/*  Formula:  counter_value  =  (4e06MH  / 16  frquency  divider)  *  Interrupt Jnterval  */ 
r  ex:  (4e06/1 6)  *0.01  seconds  =  2500  */ 

SetTimer(TIMERADDRESS_1 ,5,0x1  c61 ,2500,2500); 

Disable  lnteraipts(); 

r  Enter  the  address  of  the  interrupt  handling  routine  into  the  64th  */ 
r  entry  of  the  exception/interrupt  vector  table  of  the  CPU.  */ 
•(VectorTableBaseAddress  +  64)  =  (LONG)MotionSysHandler; 

SetLatch(0xffff0141 ,64); 

Enable  I  nterrupts(); 

} 
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E.  MOTOROLA.H 


Authors):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date:  December  8, 1993 

Revised:  March  3, 1994 

File  Name:  motorola.h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  prototypes  to  the  required  assembly 
language  routines. 

. . . . . . . / 


#ifndef _ MOTOROLA_H 

#define _ MOTOROLA_H 

#include  “definitions. h” 


Function  rexit()  is  used  to  safely  return  to  the  debugging  routine  used  on  the 
Yamabico’s  processor,  it  is  a  required  call  in  order  to  run  the  kernel  again 
without  having  to  reload  it. 


void  rexit(); 
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r 


Function  GetVectorBase{)  returns  the  starting  address  of  the  interrupt  vectors 
to  the  calling  routine.  This  is  needed  in  order  to  calculate  the  positions  to 
place  the  interrupt  handler  addresses. 


ADDRESS  GetVectorBaseQ; 


r . . . . . 

Function  SetlnterruptPriorityO  sets  the  interrupt  priority  level  to  the  value 
passed  in  as  a  parameter,  while  returning  the  old  priority  level  to  the 
calling  function.  The  return  value  can  then  be  used  to  reset  the  priority 
level  at  a  later  call.  The  parameter  must  be  a  4-byte  integer,  and  the  value 
must  be  in  the  range  0-7.  If  it  is  out  of  range,  a  -1  is  returned  to  indicate 
an  error. 


int  SetlnterruptPriority(int); 
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r~ . . . * . * . 

Interrupt  handler  _Unexpected  is  used  to  handle  the  unexpected  exceptions 

raised  during  execution  of  the  program.  Explicitly  changing  the  status  word 

is  an  example  of  such  an  exception. 

................................................................................. 
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void  UnexpectedQ; 


Interrupt  handler  JoSysHandler  is  used  to  handle  the  interrupts  from  the 
serial  port  that  is  set  up  to  handle  the  console  I/O.  It  is  really  a  shell 
routine  needed  to  call  the  C  function  that  does  the  real  work. 


void  loSysHandler(); 


r . . . * . * . .............. — ... . . . ~ 

Interrupt  handler  _MotionSysHandler  is  used  to  handle  the  interrupts  from  a 
timer  on  the  serial  board  set  for  synchronous  interrupts. 


void  MotionSysHandler(); 


#endif 


F.  MOTOROLA.ASM.S 


#  Author(s):  Scott  Book 

#  Project:  Yamabico  Robot  Control  System 

#  Date :  December  8, 1 993 

#  Revised:  March  3, 1 994 

#  File  Name:  motorola.asm.s 

#  Environment:  Sun-3  assembler  for  the  motorola  68020  processor 

#  Description:  This  file  contains  the  only  required  assembly  lanaguage  for  the 

#  MML  system.  The  main  purpose  of  this  file  is  to  define  the 

#  system  routines  that  can  not  be  defined  using  a  higher  level 

#  language. 


#  The  following  declaration  is  necessary  when  using  cc  with  the  -f68881  argument 

#  since  the  corresponding  library  is  not  linked  in.  It  can  be  eliminated  if  gcc 

#  is  used. 

.comm  f68881_used,  4 

#  Defines  the  address  of  user(). 

.globl  _user 

_user=  0x334000 

.globl  _rexit 

.globl  _GetVectorBase 

.globl  _SetlnterruptPriority 

globl  JJnexpected 
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.globl  JoSysHandler 
.gtobl  _MotionSysHandler 
.data 
.text 


#  Routine  _rexit  is  used  to  safely  return  to  the  debugging  routine  used  on  the 

#  Yamabico’s  processor. 


.even 

rexft: 

trap  #15 

.word  0x0063 

rts 


#  Routine  _GetVectorBase  returns  the  starting  address  of  the  interrupt  vectors 

#  to  the  calling  routine.  This  is  only  needed  by  higher  level  languages  that 

#  cant  read  the  vector  base  register  (vbr)  directly. 


.even 

GetVectorBase: 
link  a6,#0 
moveml  #0,sp@ 


When  entering  an  assembly  subroutine,  use  the 
link  command  to  preserve  the  previous  address 
in  the  stack  pointer.  This  makes  parameter 
passing  and  clean  up  simpler.  The  previous 
contents  of  a6  are  then  pushed  onto  the  stack. 


clrl  dO 

move  vbr.ciC 


Return  the  starting  address  of  the  vector 
interrupt  table  to  the  calling  routine. 


unlk  a6 
rts 


If  the  link  command  was  used,  ensure  that  the 
unlk  command  is  also  used  to  restore  the 
previous  contents  of  both  the  stack  pointer 
and  a6  when  the  subroutine  was  entered. 


#  Routine  _SetlnterruptPriority  sets  the  interrupt  priority  level  to  the  value 

#  passed  in  as  a  parameter,  while  returning  the  old  priority  level  to  the 

#  calling  function.  The  return  value  can  then  be  used  to  reset  the  priority 

#  level  at  a  later  call.  The  parameter  must  be  a  4-byte  integer,  and  the  value 

#  must  be  in  the  range  0-7.  If  it  is  out  of  range,  a  -1  is  returned  to  indicate 

#  an  error. 


.even 

SetlrrterruptPriority: 

link  a6,#0  |  When  entering  an  assembly  subroutine,  use  the 

moveml  dl  -d2,sp@-  j  link  command  to  preserve  the  previous  address 

j  in  the  stack  pointer.  This  makes  parameter 
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clri 

dO 

|  passing  and  clean  up  simpler.  The  previous 
|  contents  of  a6  are  then  pushed  onto  the  stack. 

clri 

dl 

j  Also  save  and  clear  any  registers  that  might 

clri 

d2 

j  be  used  in  the  routine. 

movl 

a6@(8),d2 

|  Get  the  first  parameter. 

cmpi 

#7,d2 

|  Ensure  that  the  parameter  is  between  0  and  7. 

bgt 

L10 

j  If  it's  not,  then  branch  to  the  error  area.  If 

cmpi 

#0,d2 

j  it  is,  then  place  the  value  in  the  correspond- 

bit 

L10 

|  ing  interrupt  priority  area  of  the  status 

Isll 

#8,d2 

j  register 

movw 

sr,dO 

|  Get  the  current  status  word  and  use  it  to 

movw 

dO.dl 

j  construct  the  new  status  word  with  the 

andw 

#0xF0FF,d1 

j  interrupt  priority  greater  than  or  equal  to 

orw 

d2,d1 

j  the  previous  interrupt  priority.  All  other 

movw 

dl.sr 

j  bits  remain  unchanged. 

andw 

#0x0700, dO 

|  Finally,  get  the  value  of  the  previous 

Isri 

#8,d0 

|  interrupt  priority  by  selecting  the  priority 

bra 

L20 

j  bits  and  then  return  the  value  in  register  dO. 

L10:movl 

#-1,d0 

|  An  error  has  occured,  so  return  a  negative 

L20:moveml  sp@+,d1-d2 

j  value. 

|  Restore  the  registers  that  were  saved  on 

unlk 

a6 

j  entry.  It  the  link  command  was  used,  ensure 

rts 

j  that  the  unlk  command  is  also  used  to  restore 
j  the  previous  contents  of  both  the  stack 
j  pointer  and  a6  when  the  subroutine  was 
|  entered. 

#  Interrupt  handler  .Unexpected  is  used  to  handle  the  unexpected  exceptions 

#  raised  during  execution  of  the  program.  Explicitly  changing  the  status  word 

#  is  an  example  of  such  an  exception. 


.even 

Unexpected: 

rte 


#  Interrupt  handler  JoSysHandler  is  used  to  handle  the  interrupts  from  the 

#  serial  port  that  is  set  up  to  handle  the  console  I/O.  It  is  really  a  shell 

#  routine  needed  to  call  the  C  function  that  does  the  real  work.  All  of  the 

#  system  registers  must  be  pushed  onto  the  stack  prior  to  calling  the  C  function 

#  since  the  compiler  may  or  may  not  save  the  contents  of  the  registers  prior  to 

#  use,  thus  possibly  corrupting  any  data  in  the  functions  that  were  interrupted. 
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#  The  restoration  of  the  register  must  be  done  in  reverse  order!! 


.even 

loSysHandler: 


link 

fsave 

fmovemx 

fmovel 

fmovel 

fmovel 

moveml 

a6,#-184 

a6@(-184) 

fp0-fp7,sp@- 

fpcr,sp@- 

fpsr,sp@- 

fpiar,sp@- 

d0-d7/a0-a5,sp<g>- 

|  When  entering  an  assembly  subroutine,  use  the 
j  link  command  to  preserve  the  previous  address 
j  in  the  stack  pointer.  This  makes  parameter 
|  passing  and  clean  up  simpler.  The  previous 
|  contents  of  a6  are  then  pushed  onto  the  stack, 
j  Then  save  all  of  the  system  registers. 

movel 

moveb 

#-65529,  aO 
aO@,dO 

|  Clear  the  B  control/status  register  of 
j  port  2  for  console  output  (as  per  example 
j  in  serial  pot  manual). 

jsr 

JoSysControl 

|  Call  the  C  function  that  is  the  real  work 
j  horse  of  the  interrupt  handler 

moveml 

fmovel 

fmovel 

fmovel 

fmovemx 

frestore 

unlk 

rte 

sp@+,d0-d7/a0-a5 

sp@+,fpiar 

sp@+,fpsr 

sp@+,fpcr 

sp@+,fp0-fp7 

a6@(-184) 

a6 

|  Restore  system  registers  in  reverse  order, 
j  If  the  link  command  was  used,  ensure  that  the 
j  unlk  command  is  also  used  to  restore  the 
j  previous  contents  of  both  the  stack  pointer 
j  and  a6  when  the  subroutine  was  entered. 

#  Interrupt  handler  _MotionSysHandler  is  used  to  handle  the  interrupts  from  the 

#  serial  board  timer  that  is  set  up  to  generate  synchronous  interrupts  for  motion 

#  control.  It  is  really  a  shell  routine  needed  to  call  the  C  function  that  does 

#  the  real  work.  All  of  the  system  registers  must  be  pushed  onto  the  stack  prior 

#  to  calling  the  C  function  since  the  compiler  may  or  may  not  save  the  contents 

#  of  the  registers  prior  to  use,  thus  possibly  corrupting  any  data  in  the 

#  functions  that  were  interrupted.  The  restoration  of  the  register  must  be  done 

#  in  reverse  order!! 


.even 

MotionSysHandler: 


link 

a6,#-184 

fsave 

a6@(-184) 

fmovemx 

fp0-fp7,sp@- 

fmovel 

fpcr,sp@- 

fmovel 

fpsr,sp@- 

fmovel 

fpiar,sp<5>- 

moveml 

d0-d7/a0-a5,sp@- 

jsr 

_MotionSysControl 

|  When  entering  an  assembly  subroutine,  use  the 
j  link  command  to  preserve  the  previous  address 
j  in  the  stack  pointer.  This  makes  parameter 
j  passing  and  clean  up  simpler.  The  previous 
j  contents  of  a6  are  then  pushed  onto  the  stack, 
j  Then  save  all  of  the  system  registers. 


|  Call  the  C  function  that  is  the  real  work 
j  horse  of  the  interrupt  handler 
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moveml 

sp@+,d0-d7/a0-a5 

fmovel 

sp@+,fpiar 

fmovel 

sp@+,fpsr 

fmovel 

sp@+,fpcr 

fmovemx 

sp@+,fp0-fp7 

frestore 

a6@(-184) 

unlk 

a6 

rte 

|  Restore  system  registers  in  reverse  order, 
j  If  the  link  command  was  used,  ensure  that  the 
|  unlk  command  is  also  used  to  restore  the 
j  previous  contents  of  both  the  stack  pointer 
j  and  a6  when  the  subroutine  was  entered. 


G.  SERIAL.H 

..................................................... . 

Author(s):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date:  December  8, 1993 

Revised:  March  2, 1 994 

File  Name:  serial. h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  prototypes/interface  for  the  available 
serial  system  for  the  VME8300  Quad  Serial  Port 
Board. 


#ifndef _ SERIALJH 

#define _ SERIALH 

#include  “definitions. h” 

#define  TIMERADDRESS_0  OxffffOOl  1 
#define  TIMERADDRESS_1  OxffffOl  1 1 

#define  CONSOLE  OxffffOOOl 
#define  HOST  0xffff0005 
#define  YSB1  OxffffOl  21 
#define  YSB2  OxffffOl  25 


r 


Function  ResetSerialBoardsQ  resets  both  VME8300  Quad  Serial  Port  Boards. 

. . / 


void  ResetSerialBoardsO; 


. 

Function  lnitializeConsole()  prepares  the  port  that  connects  the  onboard 
console  with  the  robot  for  I/O. 

. * . * . * . . / 
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void  lnitializeConsole(); 


Function  lnitializeHost()  sets  up  the  port  that  connects  the  Unix  work¬ 
station  with  the  robot  to  be  used  for  object  code  and  data  transfers. 

void  ln'rtializeHost(); 


/ - * . * . . . * . . 

Function  InitializePortO  establishes  the  communications  of  the  given  Port 
to  the  parameters  given.  Baud  can  be  assigned  standard  communication  rates 
300, 1200,  2400,  4800, 9600,  or  19200.  Handshaking,  Txlnterrupt,  and 
Rxlnterrupt  are  used  as  ON/OFF  flags.  An  assigned  value  of  zero  will  disable 
these  options,  while  any  non-zero  value  will  enable  them. 

void  lnitializePort(LONG  Port, 

LONG  Baud, 

LONG  Handshaking, 

LONG  Txlnterrupt, 

LONG  Rxlnterrupt); 


Function  SetTimer  sets  and  starts  a  particular  timer  to  generate  interrupts 

. * . / 

void  SetTimer(LONG  TimerAddress, 

LONG  CounterNumber, 

LONG  ModeCmd, 

LONG  LoadCmd, 

LONG  HoldCmd); 


r****************************************************************************** 
Function  SetLatchQ  passes  to  the  specified  latch  the  index  value  of  the 
vector  table  entry  containing  the  interrupt  handlers  address  to  the  serial 
board. 

void  SetLatch(LONG  LatchAddress,  BYTE  VectorNumber); 


Function  PutConsole{)  initiates  the  printing  of  the  single  character  Source 
to  the  console. 

void  PutConsole(char  Source); 


............................................................................. 

Function  PutHostQ  sends  a  single  character  to  the  port  connected  to  the  host 
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workstation. 


void  PutHost(char  Source); 


7 


. . 

Function  GetConsole()  returns  a  single  character  typed  from  the  console. 

. . 

char  GetConsole(); 


r 


Function  TerminateConsoleOutput{)  signals  the  console  port  that  the  interrupt 
generated  by  the  port  has  been  serviced. 


void  TerminateConsoleOutput(); 


7 


f  i 

/ 


Function  TerminateConsolelntCycle()  tells  the  console  port  to  terminate  all 
unserviced  interrupts. 


void  TerminateConsolelntCycleO; 


7 


#endif 


H.  SERIAL.C 

Author(s):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date:  December  8, 1993 

Revised:  March  2, 1994 

File  Name:  serial.c 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  I/O  routines  necessary  to  interface 

between  the  VME8300  Quad  Serial  Port  Board  and  the  functions  of 
the  I/O  System.  The  details  of  hardware  setup  and  initialization 
followed  the  examples  in  the  VME8300  Quad  Serial  Port  Board 
Users  Manual.  The  file  is  seperated  into  two  section.  The  first 
is  the  private  section  containing  the  encapsulated  data  and 
functions.  The  second  section  is  the  public  section.  This 
section  defines  the  interface  routines. 

WARNINGIIIThis  system  must  not  be  optimized  when  it  is  compiled.  There  are 
places  where  it  is  necessary  to  write  several  values  to  the  same 
address  in  succession.  If  an  optimization  technique  is  used,  the 
compiler  could  discard  all  but  the  last  write.  This  would  cause 
the  serial  board  to  be  improperly  setup. 
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#include  “definitions.h" 
#include  “system.h” 
#include  “motorola.h" 
#include  “iosys.h" 
#include  “serial.h" 


r 


PRIVATE  SECTION 

The  following  section  defines  the  encapsulated  definitions,  data  structures 
and  prototypes  used  in  the  system. 


7 


#def  ine  ARM  0x60 
#define  DISARM  OxcO 
#define  LOAD  0x40 

#detine  CONTROLREGISTERJ)  0 
#define  CONTROLREGISTER_1  1 
#definc  CONTr  OLREGISTER_2  2 
#define  CONTROLREGISTER_3  3 
#define  CONTROLREGISTER_4  4 
#define  CONTROLREGISTER_5  5 
#define  CONTROLREGISTER_6  6 
#define  CONTROLREGISTER  7  7 


r**************************************** 

The  following  static  function  declarations  are  the  prototypes  for  the 
encapsulated  functions. 

. . **7 

static  void  SetDataTransfer{LONG,  LONG,  LONG,  LONG); 
static  void  PutB(char  Data,  int  Memory  Address); 


Function  SetDataTnnsfer()  establishes  the  communications  for  a  given  Port 

by  setting  the  appropriate  bits  in  each  of  the  seven  control  registers  for 

that  Port.  Details  on  the  values  sent  to  the  command  and  data  registers  can 

be  found  in  the  VME8300  Quad  Serial  Board  Users  Manual. 

. . 
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static  void  SetDataTransfer(LONG  Port, 

LONG  Handshaking, 

LONG  Txlnterrupt, 

LONG  Rxlnterrupt) 

{ 

BYTE*  CmdRegister  =  (BYTE*)(Port  +  2); 

•CmdRegister  =  CONTROLREGISTER_0; 
‘CmdRegister  =  0x18; 

•CmdRegister  =  CONTROLREGISTER_2; 
•CmdRegister  =  0x00; 

•CmdRegister  =  CONTROLREGISTER_4; 
•CmdRegister  =  0x44; 

•CmdRegister  =  CONTROLR  EG  ISTE  R_1 ; 
•CmdRegister  =  0x04 1  Txlnterrupt  j  Rxlnterrupt; 

•CmdRegister  =  CONTROLREGISTERJ3; 
•CmdRegister  =  Oxcl  |  Handshaking; 

•CmdRegister  =  CONTROLREGISTER_5; 
‘CmdRegister  =  0xe8; 

•CmdRegister  =  CONTROLREGISTER_6; 
•CmdRegister  =  0x00; 

•CmdRegister  =  CONTROLREGISTER_7; 
'CmdRegister  *  0x00; 

} 


. . — ....... 

Function  PutB()  displays  the  character  in  Data  on  the  console.  It  does 
this  by  polling  the  command/status  register  to  ensure  that  the  transmitter 
buffer  is  empty  and  ready  to  receive  the  character.  The  delays  (the  for  loops) 
are  required  to  keep  the  data  from  overwhelming  the  port  and  becoming  garbage. 
NOTE: 

The  function  required  the  ‘for'  loop  as  a  timing  delay.  Without  this  delay, 
some  of  the  characters  sent  to  the  console  were  lost.  The  reason  behind  this 
data  loss  was  not  determined. 


static  void  PutB(char  Data,  int  Memory Address){ 
BYTE*  OutputData  «  (BYTE*)Memory Address; 
BYTE*  OutputCmd  *  (BYTE*)(MemoryAddress  +  2); 
const  int  DelayTime  =  0x300; 
int  Wait; 
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'OutputCmd  *  0; 

for(Wait=0;  Wait  <  DelayTime;  Wait++) 
while((*OutputCmd  &  0x04)  ==  0) 


*OutputData  =  Data; 

if(MemoryAddress  ==  CONSOLE  &&  Data  ==  \n'){ 
for(Wait=0;  Wait  <  DelayTime;  Wait++) 

while{(*OutputCmd  &  0x04)  ==  0) 


'OutputData  =  V; 

} 

} 


r 


PUBLIC  SECTION 

The  following  section  defines  the  functions  that  provide  access  to  the 
serial  sub-system. 
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r*** - ****** . . * . * - **** 

Function  ResetSerialBoards()  resets  both  VME8300  Quad  Serial  Port  Boards. 

. . 


void  ResetSerialBoards(){ 

BYTE*  Timer; 
int  Wait; 

Timer  -  (BYTE*)(TIMERADDRESS_0  +  2); 
‘Timer  «=  Oxff; 

Timer «  (BYTE*)(TIMERADDRESS_1  +  2); 
*Timer  -  Oxff; 

for(Wait=0;  Wait<0x100;  Wait++) 


} 
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. 

Function  lnitializeConsole()  prepares  the  port  that  connects  the  onboard 
console  with  the  robot  for  I/O.  It  also  establishes  the  mechanism  for 
interrupt  driven  output. 


void  lnitializeConsole(){ 
lnitializePort(CONSOLE, 9600, 0,1,0); 
SetConsolelntMechanism(); 

} 


r . * . ****** . . . — 

Function  lnitializeHost()  sets  up  the  port  that  connects  the  Unix  work¬ 
station  with  the  robot  to  be  used  for  object  code  and  data  transfers. 
. ................................................. . . / 


void  lnitializeHost(){ 
lnitializePort(HOST, 19200, 0,0,0); 

} 


r . * . . . 

Function  InitializePortO  establishes  the  communications  of  the  given  Port 
to  the  parameters  given.  Baud  can  be  assigned  standard  communication  rates 
300, 1200,  2400,  4800,  9600,  or  19200.  Handshaking,  Txlnterrupt,  and 
Rxlnterrupt  are  used  as  ON/OFF  flags.  An  assigned  value  of  zero  will  disable 
these  options,  while  any  non-zero  value  will  enable  them. 


void  lnrtializePort(LONG  Port, 
LONG  Baud, 

LONG  Handshaking, 
LONG  Txlnterrupt, 
LONG  Ftxlnterrupt) 

{ 

LONG  taddr; 

LONG  cnum; 

LONG  Icount; 

LONG  hcount; 

switch  (Port){ 
case  CONSOLE: 
taddr  =  TIMERADDRESS_0; 
cnum  -  1 ; 
break; 
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case  HOST : 

taddr  -  TIMERADDRESS_0; 
cnum  ■  2; 
break; 
case  YSB1 : 

taddr  =  TIMERADDRESS_1 ; 
cnum  *  3; 
break; 
case  YSB2: 

taddr  =  TIMERADDRESS_1 ; 
cnum  =  4; 
break; 
default: 

PutStr(“i_Port:  illegal  porT); 
rexit(); 

} 

switch  (Baud){ 
case  300: 

(count  =  0x01  aO; 
hcount  =  0x01 al; 
break; 
case  1200: 

(count  =  0x0068; 
hcount  =  0x0068; 
break; 
case  2400: 

(count  =  0x0034; 
hcount  -  0x0034; 
break; 
case  4800: 

Icount  *  0x001  a; 
hcount  *  0x001  a; 
break; 
case  9600: 

I count  -  OxOOOd; 
hcount  *  OxOOOd; 
break; 
case  19200: 

Icount  =  0x0006; 
hcount  -  0x0007; 
break; 
default: 

PutStrfi _port:  bad  baud  rate"); 
rexit(); 


SetTimer(taddr,  cnum,  0x0b62,  Icount,  hcount); 

SetDataT  ransfer(Port, 

(Handshaking  ?  0x20  : 0), 
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} 


(Txlnterrupt  ?  0x02  :  0), 
(Rxlnterrupt  ?  0x10  : 0)); 


Function  SetTimer()  initializes  one  of  the  five  counters  located  on  the 
serial  board  (with  the  corresponding  TimerAddress)  by  assigning  values  to 
command  and  data  registers  for  that  timer.  Details  on  the  values  sent  to  the 
command  and  data  registers  can  be  found  in  the  VME8300  Quad  Serial  Board 
Users  Manual. 


void  SetTimer(LONG  TimerAddress, 

LONG  CounterNumber, 

LONG  ModeCmd, 

LONG  LoadCmd, 

LONG  HoldCmd) 

( 

BYTE*  CmdRegister  =  (BYTE*)(TimerAddress  +  2); 

BYTE*  Daiaftegister  *  (BYTE*)(TimerAddress); 

BYTE  CounterSelectBit; 

BYTE  TimerCommand; 

CounterSelectBit  =  1 ; 

CounterSelectBit  =  CounterSelectBit «  (BYTE)(CounterNumber  - 1); 

TimerCommand  =  DISARM; 

TimerCommand  *  TimerCommand  |  CounterSelectBit; 

'CmdRegister  =  TimerCommand; 

TimerCommand  =  LOAD; 

TimerCommand  =  TimerCommand  |  CounterSelectBit; 

‘CmdRegister  *  TimerCommand; 

‘CmdRegister  =  (BYTE)CounterNumber; 

‘DataRegister  =  (BYTE)ModeCmd; 

‘DataRegister  =  (BYTE)(ModeCmd  »  8); 

‘DataRegister  =  (BYTE)  LoadCmd; 

‘DataRegister  =  (BYTE)(LoadCmd  »  8); 

‘DataRegister » (BYTE)HoldCmd; 

‘DataRegister  =  (BYTE)(HoldCmd  »  8); 

TimerCommand  =  ARM; 

TimerCommand  *  TimerCommand  |  CounterSelectBit; 

‘CmdRegister  =  TimerCommand; 

} 
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Function  SetLatch()  passes  to  the  specified  latch  the  index  value  of  the 
vector  table  entry  containing  the  interrupt  handlers  address  to  the  serial 
board.  When  an  interrupt  associated  with  the  given  latch  is  generated,  the 
VectorNumber  is  placed  onto  the  address  bus,  indicating  to  the  cpu  the  vector 
table  entry  that  contains  the  address  of  the  interrupt  handler. 


void  SetLatch(LONG  LatchAddress,  BYTE  VectorNumber){ 
*(BYTE*)Latch  Address  *  VectorNumber; 

) 


Function  PutConsole()  initiates  the  printing  of  the  single  character  Source 
to  the  console. 


void  PutConsole(char  Data){ 
PutB(Data.CONSOLE); 

} 


r****************************************************************************** 

Function  PutConsole()  initiates  the  printing  of  the  single  character  Source 
to  the  host  system. 

. * . / 


void  PutHost(char  Data){ 
PutB(Data.HOST); 

) 


r — *••♦«**••***••*******..**•.*..** . ****** 

Function  GetConsole()  polls  the  command/status  register,  waiting  until  a 
character  from  the  console  is  placed  in  the  receive  bufler.  It  then  returns 
that  character  to  the  calling  function. 

. . * . / 


char  GetConsole(){ 

BYTE*  InputAddress  -  (BYTE*)CONSOLE ; 
BYTE*  InputCmd  -  (BYTE*)(CONSOLE  +  2); 

‘InputCmd  =  0; 
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while({*lnputCmd  &  0x01)  «=«=  0) 
> 

return  *(BYTE*)lnputAddress; 

} 


/*** . . . * . * . . . * . . . 

Function  TerminateConsoleOutput()  signals  the  console  port  that  the  interrupt 
generated  by  the  port  has  been  serviced. 

. . * . . . . . ***** . / 


void  TerminateConsoleOutput(){ 
*(BYTE*)(CONSOLE  +  2)  =  0x28; 

) 


r*** . * . * . ****** . 

Function  TerminateConsolelntCycle()  tells  the  console  port  to  terminate  all 
unserviced  interrupts. 


void  TerminateConsolelntCycle(){ 
*(BYTE*)(CONSOLE  +  2)  =  0x38; 

) 
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APPENDIX  B 


A.  IOSYS.H 

Author(s):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date:  December  8, 1993 

Revised:  March  2, 1994 

File  Name:  iosys.h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  prototypes/interface  for  the  functions 
available  in  the  I/O  System  module. 

. . * . ***** . * . ****** . / 


#ifndef _ IOSYS_H 

#define  _IOSYS_H 
#include  “definitions.h,' 


Function  loSyslnitialize{)  initializes  the  ports  used  for  serial  communications. 

............................................................................ 

void  loSyslnitialize(); 


r 


Function  loSysControl()  is  the  work-horse  routine  for  the  output  interrupt 
handler. 


void  loSysControlQ; 


r 


Function  PutStr()  initiates  the  printing  of  the  characters  in  the  Source 
string  to  the  console. 


void  PutStr(char*  Source); 


r 


Function  Putlnt  sends  the  ascii  representation  of  the  parameter  Number  to 
the  console. 


void  Putlnt(int  Number); 


V 
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r....................................................... . ................... 

Function  PutReal  ser,ds  the  ascii  representation  of  the  parameter  Number  to 
the  console.  The  output  is  in  exponential  notation  with  a  total  of  Places 
digits  after  the  decimal  (up  to  a  maximum  of  the  constant  MAX_REAL_PRECISION 
found  in  the  source  file). 

................................................................................. 


void  PutReal(double  Number,  int  Places); 


Function  GetStr()  copies  the  string  of  characters  typed  at  the  console  (up  to 

Length  characters)  into  the  string  pointed  at  by  Source. 

. * . .................. . 


void  GetStr(char*  Source,  int  Length); 


Function  Getlnt()  reads  a  string  from  the  console  (up  to  MAX_INTEGER_DIGITS) 
and  converts  the  ascii  representation  into  its  integer  value.  All  leading 
white  space  (spaces  and  tabs)  are  discarded.  The  conversion  stops  at  the 
first  character  that  can  not  be  part  of  a  legel  integer  value.  All  remaining 
characters  in  the  string  are  discarded.  Strings  where  the  first  non  white- 
space  character  is  not  a  digit  will  default  to  0.  Empty  strings  also  default 
to  0. 

. * . . . .. . / 


int  Getlnt(); 


Function  GetRealQ  reads  a  string  from  the  console  (up  to  MAX_REAL_PRECISION) 

and  converts  the  ascii  representation  into  its  double  value.  All  leading 

white  space  (spaces  and  tabs)  are  discarded.  The  conversion  stops  at  the 

first  character  that  can  not  be  part  of  a  legel  double  value.  All  remaining 

characters  in  the  string  are  discarded.  Strings  where  the  first  non  white- 

space  character  is  not  a  digit  will  default  to  0.0.  Empty  strings  also 

default  to  0. 

. . ***/ 


double  GetRealQ; 


#endif 


B.  IOSYS.C 

/**•* . * . . 

Author(s):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date:  December  8, 1993 
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Revised:  March  2, 1994 
File  Name:  iosys.c 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  routines  and  data  structures  needed  to 
provide  I/O  capabilities  to  the  rest  of  the  subsystems  in  the 
Yamabico  project.  This  file  contains  two  marked  sections.  The 
first  is  the  private  section  containing  the  encapsulated  data 
and  functions.  The  second  section  is  the  public  section.  This 
section  defines  the  interface  routines. 


#include  “definitions. h” 
#include  “convertutil.h” 
#indude  “serial. h” 
#include  “iosys.h" 


r 


PRIVATE  SECTION 

The  following  section  defines  the  encapsulated  definitions  and  data 
structures  used  in  the  system. 
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#define  BUFSIZE  1024 

Structure  and  declaration  of  the  output  buffer.  It  is  declared  static  to 
prevent  access  from  routines  external  to  this  module. 

typedef  structj 
int  Head: 
int  Tail: 
int  Count; 

char  Buffer{BUFSIZE]; 

}  IOBUFFER; 


static  IOBUFFER  OutputBuf; 


r 


PUBLIC  SECTION 
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The  following  section  defines  the  functions  that  provide  access  to  the 
terminal  system. 
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...................................................................... 

Function  loSyslnitialize()  initializes  the  ports  used  for  serial  communications. 
The  CONSOLE  port  is  set  for  interrupt  driven  output,  but  direct  memory  (polled) 
input.  The  output  buffer  is  also  initialized. 


void  loSyslnitialize(){ 
OutputBuf.Head  =  0; 
OutputBuf.Tail  =  0; 
OutputBuf.Count  =  0; 

} 


Function  loSysControl()  controls  the  printing  of  characters  in  the  output 

buffer  to  the  screen.  It  is  called  from  the  interrupt  handling  routine.  It 

operates  by  printing  one  character  to  the  screen  and  then  terminating  the 

interrupt.  The  act  of  printing  a  character  to  the  screen  generates  another 

interrupt  to  print  the  next  character  in  the  buffer.  If  the  buffer  is  empty, 

it  sends  a  command  to  the  port  to  stop  the  interrupt  chain. 

............................................................................ 


void  loSysControl(){ 

if(OutputBuf.Count  >  0){ 

PutConsole(Out  putBuf.Bufferf  OutputBuf.Tail]); 
OutputBuf.Count--; 
if  (OutputBuf.Tail  ==  BUFSIZE-1){ 
OutputBuf.Tail  =  0; 

}else 

OutputBuf.Tail++; 

}else 

T  erminateConsoleOutput() ; 
TerminateConsolelntCycleQ; 

} 


Function  PutStr  copies  the  string  pointed  to  by  Source  into  the  output  buffer. 
A  critical  region  exists  where  the  number  of  characters  in  the  output  buffer 
is  incremented.  Therefore,  the  priority  mask  is  set  to  prevent  any  interrupts 
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from  taking  control  of  the  CPU  during  its  execution. 


7 


void  PutStr(char*  Source){ 
while  (‘Source  I-  *\0’){ 

Output  Buf.Count++; 

OutputBuf.BuffeitOutputBuf.Head]  =  ‘Source; 
Source++; 

rf(OutputBuf.Head  «  BUFSI2E-1){ 
OutputBuf.Head  -  0; 

Jelse 

OutputBuf.Head++; 


/*  Initiate  an  output  intemjpt  by  sending  a  null  character  to  the  console.  */ 
PutConsoleOO'); 

} 


r . * . 

Function  Putlnt  converts  the  parameter  Number  to  its  ascii  representation  and 
sends  the  resulting  string  to  the  console  by  calling  to  PutStr. 
*******************“***“***************“***•************••*******“***•****“/ 

void  Putlnt(int  Number){ 
char  NumStr(20]; 

ItoA(Number.NumStr); 

PutStr(NumStr); 

} 


r . * . * . * . * . 

Function  PutReal  converts  the  exponential  notation  of  the  parameter  Number 
into  its  ascii  representation.  It  then  sends  the  resulting  string  to  the 
console  by  calling  PutStr. 

. * . . . . . . . ! 


void  PutRea!(double  Number,  int  Places)} 
char  NumStr[MAX_REAL_PRECISION+7]; 

H(Places<MAX_REAL_PRECISION){ 

RtoAE(Number,NumStr,Places+1); 

}else{ 

RtoAE{Number,NumStr,MAX_REAL_PRECISION+1); 

} 
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PutStr(NumStr); 


} 


Function  GetStr()  copies  the  string  of  characters  typed  at  the  console  (up  to 

Length  characters)  into  the  string  pointed  at  by  Source. 

. . 


void  GetStr(char*  Source,  int  Length){ 
BYTE  Keystroke; 
int  I  =  -1 ; 

int  Size  =  Length  - 1 ; 
do{ 

Keystroke  =  GetConsole{); 

switch(KeyStroke){ 
case  ‘\b‘: 
if  (I  >-  0){ 

I--; 

PutConsoie(KeyStroke); 

} 

break; 

default: 

Source[++ll  =  Keystroke; 
PutConsole(KeyStroke); 

} 

)while((Source[l]  I-  V)  &&  (I  <  Size)); 
Source[l] » “NO'; 

} 


r . * . ***** . * . * . * . * . . 

Function  Getlrrt()  reads  a  string  from  the  console  (up  to  MAX_INTEGER_DIGITS) 
and  converts  the  ascii  representation  into  its  integer  value.  All  leading 
white  space  (spaces  and  tabs)  are  discarded.  The  conversion  stops  at  the 
first  character  that  can  not  be  part  of  a  legel  integer  value.  All  remaining 
characters  in  the  string  are  discarded.  Strings  where  the  first  non  white- 
space  character  is  not  a  digit  will  default  to  0.  Empty  strings  also  default 
to  0. 

. ***** . * . / 


int  Getlnt(){ 

char  NumStrfMAX_INTEGER_DIGITS+1]; 
char*  Temp  *  NumStr; 
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GetStr(NumStr,MAX_INTEGER_DIGITS+1); 

while ((*Temp==Y)  ||  (*Temp==' ')) 

Temp++; 

return  Atol(Temp); 

} 


........................................................................ 

Function  GetReal()  reads  a  string  from  the  console  (up  to  MAX_REAL_PRECISION) 

and  converts  the  ascii  representation  into  its  double  value.  All  leading 

white  space  (spaces  and  tabs)  are  discarded.  The  conversion  stops  at  the 

first  character  that  can  not  be  part  of  a  legel  double  value.  All  remaining 

characters  in  the  string  are  discarded.  Strings  whore  the  first  non  white- 

space  character  is  not  a  digit  will  default  to  0.0.  Empty  strings  also 

default  to  0. 

. “***♦*♦'**•/ 


double  GetReal(){ 

char  NumStr[MAX_REAL_PRECISION+1]; 
char*  Temp  =  NumStr; 

GetStr(NumStr,MAX_REAL_PRECISION+1); 

while(rTemp==’\t')  ||  (*Temp=='  ‘)) 

Temp++; 

return  AtoR(Temp); 

} 


APPENDIX  C 


A.  MOTION.H 


Autho  r(s) :  Scott  Book 
Project:  Yamabico  Robot  Control  System 
Date:  December  16, 1993 

Revised:  March  2, 1994 
File  Name:  motion. h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  prototypes/interface  for  the  functions 
available  in  the  Motion  System  module.  The  first  two  routines 
are  for  system  setup  and  control.  The  rest  are  MML’s  immediate 
commands  defined  by  the  language. 

#ifndef  _MOTION_H 
#define  _MOTION_H 
#include  “definitions. h" 


Function  MotionSyslnitialize{)  initializes  the  motion  subsystem  by  assigning 
default  values  to  the  local  variables  and  establishing  the  interrupt  handling 
mechanism. 

. / 


void  MotionSyslnitialize(); 


r — . * 

Function  MotionSysControl()  is  the  interrupt  handler  workhorse  and  is  called 
from  the  assembly  interrupt  handler  shell. 
. / 


void  MotionSysControlQ; 


r*********** . * . ****** . . ****** 

The  following  declarations  are  prototypes  for  MML's  immediate  commands. 
The  command  descriptions  can  be  found  in  the  MML  Bible. 

. * . / 

void  SetRobotConfiguration(CONFIGURATION  NewConfiguration); 

void  GetRobotConfiguration(CONFIGU RATION*  CurrentConfiguration); 

void  Stop(); 
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void  SetLinearVelodty(double  LinearVelocity); 

void  SetRotationa!Velocity(double  RotationalVelocity); 

void  SetLinearAcceleration(doubie  LinearAcceleration); 

void  SetRotationalAcceleration(double  RotationalAcceleration); 

void  SetSizeConstant(double  SizeConstant); 

double  GetTotalDistance(); 

void  SkipPathElemerrt(); 

void  HaltMotionQ; 

void  ResumeMotion(); 

CONFIGURATION  SetlnitialPosition(); 

void  ReportRobotConfiguration(CONFIGURATION  CurrentConfiguration); 
void  MotionOn(); 
void  MotionOff(); 

#endif 


B.  MOTION.C 

.......................................................................... 

Authors):  Scott  Book 
Project:  Yamabico  Robot  Control  System 
Date:  December  16. 1993 

Revised:  March  2, 1994 
File  Name:  motion.c 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  provides  the  routines  and  data  structures  needed  to 
provide  the  motion  control  capability  for  the  robot.  The  file 
is  divided  into  three  sections.  The  first  is  the  private  section 
containing  the  encapsulated  data  and  functions.  The  second 
section  is  the  control  section.  This  section  defines  the 
routines  required  for  motion  control.  The  third  section  is  the 
Immediate  command  section.  This  section  defines  MML’s  immediate 
commands.  The  routines  in  these  last  two  sections  can  be 
accessed  publicly. 

. ******* . ****/ 


#include  “definitions. h" 
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#include  “system,  h” 
#include  “iosys.h” 
#include  “wheels. h" 
#include  “math.h" 
include  “math68881.h” 
#include  “motiontrace.h" 
#include  “motion.h” 


r 


PRIVATE  SECTION 

The  following  section  defines  the  encapsulated  definitions,  data  structures 
and  prototypes  used  in  the  system. 
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#define  SMALLERROR  0.0001 

static  int  LoopTest; 

static  BOOLEAN  Halted; 

static  VELOCITY  HaltedVelocity; 
static  VELOCITY  OesiredVetocity; 
static  VELOCITY  Commanded; 
static  VELOCITY  DesiredAcceleration; 

static  double  TotalDistance; 

static  double  DesiredSizeConstant; 

static  CONFIGURATION  VehicleConfiguration; 

........................................................ . . 

The  following  static  function  declarations  are  the  prototypes  for  the 
encapsulated  functions. 

static  void  UpdateConfiguration(double  DeltaDistanceChanged, 
double  DeltaOrientation); 

static  VELOCITY  GetCommandedVelocity(VELOCITY  Desired, 
VELOCITY  Actual, 

VELOCITY  Commanded); 

static  double  GetLinearVelocity{double  DesiredVelocity, 
double  ActualVelocity, 
double  LastCommandedVelocity); 
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— ......... 

Function  UpdateConfiguration()  calculates  the  robots  current  position  based 
on  DeltaDistanceChanged  (ds)  and  DeltaOrientation  (dt). 

................................. - ...........................................y 


static  void  UpdateConfiguration(double  DeltaDistanceChanged, 
double  DeltaOrientation){ 

double  Distancelncrement  =  DeltaDistanceChanged; 
double  Orientationlncrement  =  DeltaOrientation  /  2; 

Disable  Interrupts!); 

if(fabs(DeltaOrientation)  >  SMALLERROR) 

Distancelncrement  *=  sin(Orientationlncrement)  /  Orientationlncrement; 

VehicleConfiguration.Position.XPosrtion  +=  Distancelncrement  * 

cos(VehicleConfiguration.Orientation  +  Orientationlncrement); 
VehicleConfiguration.Position.YPosition  +=  Distancelncrement  * 

sin(VehicleConfiguration.Orientation  +  Orientationlncrement); 
VehicleConfiguration.Orientation  +=  DeltaOrientation; 
if(fabs(DeltaDistanceChanged)  >  SMALLERROR){ 
VehicleConfiguration.Kappa  =  DeltaOrientation  /  DeltaDistanceChanged; 
}else{ 

VehicleConfiguration.Kappa  «  DeltaOrientation  /  SMALLERROR; 

} 

Enablelnterrupts(); 

) 


y......................................... . . 

Function  GetCommandedVelocityO  calculates  the  commanded  velocity  based  on 
the  current  velocity,  the  desired  velocity,  and  the  previous  commanded 
velocity. 

. . .......y 


static  VELOCITY  GetCommandedVelocity(VELOCITY  Desired, 
VELOCITY  Actual, 

VELOCITY  Commanded)! 

Commanded. Linear  - 

GetLinearVelocity(Desired.Linear,  Actual.  Linear,  Commanded.  Linear); 

r  This  statement  is  used  since  GetRotationalVelocity()  is  not  7 
r  currently  defined.  Otherwise,  a  statement  similar  to  above  V 
r  would  be  used.  7 
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Commanded. Rotational  =  Desired. Rotational; 


return  Commanded; 

} 


Function  GetLinearVelocityO  calculates  the  linear  component  of  the  commanded 
velocity. 


static  double  GetLinearVelocity(double  DesiredVelocity, 
double  ActualVelocity, 
double  CommandedVelocity){ 
double  VelocityChange; 

VelocityChange  *  DesiredAcceleration.Linear  *  MOTION_CONTROL_CYCLE; 
if(ActualVelocity  <  DesiredVelocity){ 

CommandedVelocity  -  Min(CommandedVelocity  +  VelocityChange,  DesiredVelocity); 
}else{ 

CommandedVelocity  =  Max(CommandedVelocity  -  VelocityChange,  DesiredVelocity); 

} 

return  CommandedVelocity; 

} 


r 


MOTION  CONTROL  SECTION 

The  following  section  defines  the  functions  that  provide  access  to  the 
motion  control  system.  These  routines  are  public. 
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Function  MotionSysInitializeQ  initializes  all  of  the  private  global  variables 
in  this  module  to  the  desired  default  values.  It  then  calls  SetTimer  to 
program  the  5th  timer  on  serial  board  #1  (the  second  serial  board)  to  generate 
synchronous  interrupts  every  10ms.  After  the  timer  has  been  set  up,  the 
interrupt  handling  routine  is  made  available  to  the  system  by  the  call  to 
SetMotionlnterruptHandler(). 

. •♦•••••••••‘•♦••••♦♦•***********‘*****‘»*«* . ***7 


void  MotionSyslnitialize(){ 
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LoopTest  *  0; 

Halted  -  FALSE; 

TotalDistance  »  0.0; 

DesiredVelocity.Linear  =  0.0; 

Desired  Velocity.  Rotational  «=  0.0; 

Commanded.Linear  *  0.0; 

Commanded.Rotational  =  0.0; 

DesiredAcceleration.Linear  =  20.0; 

DesiredAcceleration. Rotational  =  0.5; 

DesiredSizeConstant  =  20.0; 

VehicleConfiguration.Position.XPosition  *  0.0; 
VehicleConfiguration.Position.YPosition  =  0.0; 
VehicleConfiguration.Orientation  =  0.0; 

VehicleConfiguration.Kappa  «=  0.0; 

I*  Initialize  sub-systems.  7 
Initialize  Wheels(); 

TraceMotionSyslnitialize(400);  /*  This  sub-system  only  required  if  data  7 
1*  logging  is  desired.  7 

SetMotionlntMechanism(); 

} 


r . * . 

Function  MotionSysControl{)  is  the  interrupt  handler  workhorse.  It  is  called 
from  the  assembly  interrupt  handler  shell.  Its  first  task  is  to  update  the 
change  in  position  and  orientation  through  calls  to  the  module  responsible  for 
movement.  It  then  uses  this  information  in  the  motion  control  laws  to  derive 
the  commanded  linear  and  rotational  velocities  required  for  this  motion 
control  cycle.  Finally,  it  passes  these  computed  velocities  back  to  the  move¬ 
ment  module  for  execution. 


void  MotionSysControl(){ 
double  OrientationChange; 
double  DistanceChanged; 

VELOCITY  Actual; 

UpdateMovemerrtO; 

DistanceChanged  -  GetDistanceTraveled(); 
OrientationChange  =  GetOrientationChange(); 

TotalDis^nce  +=  DistanceChanged; 
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UpdateConfiguration(DistanceChanged,OrientationChange); 

Actual. Linear  =  DistanceChanged  /  MOTION_CONTROL_CYCLE; 

Actual. Rotational  =  OrientationChange  /  MOTION_CONTROL_CYCLE; 

r  The  logging  statement  can  be  moved,  modified  or  deleted  as  desired.  7 
LogTimedMotion(  Actu  al .  Linear) ; 

Commanded  =  GetCommandedVelocity(DesiredVelocity,  Actual,  Commanded); 

SetMovement(Commanded. Linear, Commanded. Rotational); 

r  LoopTest  used  to  control  output  from  interrupt  driven  motion  control  7 
r  system.  LoopTest  is  assigned  zero  every  100  cycles  (1  sec).  7 
if  (LoopTest ++  >=  99) 

LoopTest  =  0; 

) 


r 


IMMEDIATE  COMMAND  SECTION 

The  following  section  defines  the  functions  that  provide  access  to  MML’s 
immediate  commands.  The  functionality  of  these  routines  can  be  found  in  the 
language  definition.  These  routines  are  also  public. 
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void  SetRobotConfiguration(CONFIGURATION  NewConfiguration){ 

Disable  lnterrupts(); 

VehicleContiguration.  Position. XPosition  =  NewConfiguration.Position.XPosition; 
VehicleConfiguration.Position.YPosition  =  NewConfiguration.Position.YPosition; 
VehicleContiguration.  Orientation  =  NewConfiguration. Orientation; 
VehicleConfiguration.Kappa  *  NewConfiguration. Kappa; 

Enablelnterrupts(); 

} 


void  GetRobotConfiguration(CONFIGURATION*  CurrentConfiguration){ 
Disable  lnterrupts(); 

*CurrentConfiguration  -  VehicleConfiguration; 

Enable  I  nterrupts(); 

) 
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void  Stop{){ 

Wheels  Disable  (); 

DesiredVelocity  .Linear  -  0.0; 
DesiredVelocity.  Rotational  =  0.0; 

} 


void  SetLinearVelocity(double  LinearVelocity){ 
DesiredVelocity.Linear  =  LinearVelocity; 

) 


void  SetRotationalVelocity(double  RotationalVelocity){ 
DesiredVelocity. Rotational  *  RotationalVelocity; 

i 


void  SetLinearAcceleration{double  LinearAcceleration){ 
DesiredAcceleration. Linear  -  LinearAcceleration; 

} 


void  SetRotationalAcceleration(double  RotationalAcceleration){ 
DesiredAcceleration. Rotational «  RotationalAcceleration; 

} 


void  SetSizeConstarrt(double  SizeConstant){ 
DesiredSizeConstant  -  SizeConstant; 

} 


double  GetTotalDistanceO{ 
return  TotalDistance; 

} 
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void  SkipPathElement(){ 
PutStr(Anlnside  skip()  stub”); 

} 


void  HattMotion(){ 
if(IHalted){ 

Halted  =  TRUE; 

HaltedVelocity  .Linear  =  DesiredVelocity.Linear; 
HaltedVelocity.Rotational  =  DesiredVelocity.Rotational; 
WheelsDisable(); 

} 

} 


void  ResumeMotion(){ 
if(Halted){ 

Halted  =  FALSE; 

DesiredVelocity.Linear »  HaltedVelocity.Linear; 

D esired Velocity . Rotation al  =  HaltedVelocity  .Rotational; 
WheelsEnable(); 

} 

} 


CONFIGURATION  SetlnitialPositionO{ 
CONFIGURATION  NewConfiguration; 

PutStrftnEnter  the  Starting  X  Position:  “); 
NewConfiguration.  Position  .XPosition  =  GetRealO; 
PutStr(“Enter  the  Starting  Y  Position:  “); 
NewConfiguration.Position.YPosition  -  GetReal(); 
PutStr(“Enter  the  Starting  Orientation:  “); 
NewConfiguration.Orientation  *=  GetRealO; 
PutStrfEnter  the  Starting  Kappa  Value:  “); 
NewConfiguration.Kappa  -  GetRealO; 

return  NewConfiguration; 

) 
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void  ReportRobotConfiguration(CONFlGURATION  CurrentConfiguration){ 
PutStr(“\nCurrent  Robot  Configuration:\n\tX  =>\i\T); 
PutReal(CurrentConfiguration. Posit  ion.XPos'ition.4); 

PutStr(“\n\tY  *>\t\t”); 

PutReal(CurrentConfiguration.Position.YPosition,4); 

PutStr(“\n\tTheta  «>\t"); 

PutReal(CurrentConfiguration.Orientation,4); 

PutStr("\n\tKappa  =>\T); 

PutReal(CurrentCorrfiguration.Kappa,4); 

} 


void  MotionOn(){ 
WheelsEnable(); 

} 


void  MotionOffO{ 
WheelsDisableO; 

} 


C.  MOTIONTRACE.H 


Authors):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date:  January  20, 1994 

Revised:  March  3, 1994 

File  Name:  motiontrace.h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  prototypes/interface  for  the  functions 
available  to  log  motion  control  data. 


tifndef _ MOTIONTRACE.H 

#define _ MOTIONTRACE.H 


. ......................... 

Function  TraceMotionSyslnitialize()  prepares  the  tracing  system  to  log  data. 

It  requests  a  block  of  dynamic  memory  to  store  the  number  of  data  points 
requested  by  NumberOfPoints.  No  error  checking  for  dynamic  memory  allocation 
is  performed  by  this  function. 
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void  TraceMotionSyslnitialize(int  NumberOfPoints); 


7 


r...................................................... . 

Function  MotionTraceEnable()  enables  data  logging.  Frequency  is  used  by  the 

system's  logging  functions  to  determine  the  number  of  motion  control  cycles 

between  logged  data.  For  example,  H  the  frequency  is  3,  then  data  would  be 

logged  on  every  third  call  to  LogTimedMotionQ  or  LogMotionData(). 

................................................................................. 


void  MotionTraceEnable(int  Frequency); 


r................................................................... — ........ 

Function  MotionTraceDisable()  disables  data  logging. 

. . . . / 

void  MotionTraceDisable(); 


Function  LogTimedMotion()  logs  Data  against  time  in  seconds,  starting  when 

data  logging  is  turned  on.  This  is  based  the  assumption  that  the  routine  is 

called  every  motion  control  cycle.  The  Frequency  value  given  when  logging  is 

enabled  determines  the  number  of  number  of  LogTimedMotion()  calls  required 

between  recorded  data. 

******* . 


void  LogTimedMotion(double  Data); 


r........................................................... . . 

Function  LogMotionData()  logs  both  parameters  after  every  ‘Frequency’  calls. 


void  LogMotionData  (double  XPIot,  double  YPIct); 


. 

Function  DownLoadMotionData()  prompts  the  user  for  a  file  name  on  the  host 
system  to  store  the  data.  The  file  then  opened,  deleting  any  previous 
contents.  The  routine  then  transfers  character  string  representations  of  the 

recorded  data  to  the  Unix  host. 

. . . 


void  DownLoadMotionData(); 


#endif 
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D.  MOTIONTRACE.C 


r 


Author(s):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date:  January  20, 1994 

Revised:  March  3, 1994 

File  Name:  motiontrace.c 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  routines  required  to  for  the  tracing 
system.  The  file  is  seperated  into  two  section.  The  first  is 
the  private  section  containing  the  encapsulated  data  and 
functions.  The  second  section  is  the  public  section.  This 
section  defines  the  interface  routines. 

. * . * . . . / 


#include  “definitions.h” 
#include  “memsys.h" 
#include  “convertutil.h” 
#include  “serial.h” 
#include  “iosys.h" 
#include  “motiontrace.h" 


r 


PRIVATE  SECTION 

The  following  section  defines  the  encapsulated  definitions,  data  structures 
and  prototypes  used  in  the  system. 
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typedef  structf 
double  XData; 
double  YData, 

}  SAMPLINGPOINT; 

static  SAMPLINGPOINT  ‘MotionData; 

static  int  MaxDataPoints; 
static  int  NextPlot; 
static  int  LoggingData; 
static  int  CycleCounter; 
static  int  LoggingFrequency; 


/...................................o...... . . 

The  following  static  function  declarations  are  the  prototypes  for  the 
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encapsulated  functions. 


static  void  PutStrHost(char*  Source); 


7 


r........... - ............................................................ 

Function  PutStrHost()  is  a  simple  routine  that  writes  a  character  string  to 
the  host  environment.  Writing  non-character  information  to  the  host 
environment  may  produce  unexpected  results.  Other  routines  may  be  created  to 
write  non-character  data  by  converting  the  data  to  character  strings  prior 
to  being  sent. 

............................................. . ....................... 


static  void  PutStrHost(char*  Source){ 
while  ('Source  !=  ‘\0’){ 

PutHost('Source); 

Source++; 

} 


r 


PUBLIC  SECTION 

The  following  section  defines  the  functions  that  provide  access  to  the 
tracing  sub-system. 
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. . . . 

Function  TraceMotionSyslnitialize()  prepares  the  tracing  system  to  log  data. 

It  requests  a  block  of  dynamic  memory  to  store  the  number  of  data  points 
requested  by  NumberOfPoints.  No  error  checking  for  dynamic  memory  allocation 
is  performed  by  this  function. 

. . . . . . . . . . v 


void  TraceMotionSyslnitialize(int  NumberOfPoints){ 
NextPlot  *  0; 

LoggingData  «  FALSE; 

CycleCounter  *=  0; 

LoggingFrequency  =  1 ; 
if(NumberOfPoints  >  0) 

MaxDataPoints  ■  NumberOfPoints; 
else 

MaxDataPoints  =  0; 
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free((BYTE*)MotionData); 

MotionData  ■  (SAMPLINGPOINT*)malloc(sizeof(SAMPLINGPOINT)*NumberOfPoints); 

} 


Function  MotionTraceEnable()  enables  data  logging.  Frequency  is  used  by  the 
system's  logging  functions  to  determine  the  number  of  motion  control  cycles 
between  logged  data.  For  example,  if  the  frequency  is  3,  then  data  would  be 
logged  on  every  third  call  to  LogTimedMotionQ  or  LogMotionData(). 
******************************************************************‘***********/ 


void  MotionTraceEnable(int  Frequency){ 
if(Frequency  >  0) 

LoggingFrequency  =  Frequency; 
else 

LoggingFrequency  =  1 ; 

LoggingData  =  TRUE; 

} 


r***** . * . * . * . ***** . * . ***** . ******** 

Function  MotionTraceDisableQ  disables  data  logging. 

. . . * . * . *******/ 

void  MotionTraceDisable(){ 

LoagingData  «=  FALSE; 

} 


r . * . * . * .  ********** 

Function  LogTimedMotion{)  logs  Data  against  time  in  seconds,  starting  when 
data  logging  is  turned  on.  This  is  based  the  assumption  that  the  routine  is 
called  every  motion  control  cycle.  The  routine  uses  the  variable  CycleCounter 
as  a  counter.  When  the  value  of  CycleCounter  reaches  zero,  data  is  logged  and 
the  counter  is  reset  to  the  LoggingFrequency  set  when  logging  was  enabled.  If 
the  value  is  greater  than  zero,  the  counter  is  decremented  and  logging  is  NOT 
performed. 

. . 


void  LogTimedMotion(double  Data){ 
if(LoygingData){ 

rf((CycleCounter~  <-  0)  &&  (NextPlot  <  MaxDataPoints)){ 
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MotionData[NextPlot].XData  =  NextPlot  *  LoggingFrequency 
*  MOTION_CONTROL_CYCLE; 
MotionData[NextPlot].YData  =  Data; 

NextPlot++; 

CycleCounter  =  LoggingFrequency; 

} 

} 

} 


Fucntion  LogMotionData()  records  both  parameters  when  the  CycleCounter  is 
zero.  It  then  resets  the  counter  to  LoggingFrequency,  assigned  when  logging 
is  enabled.  If  the  value  is  greater  than  zero,  the  counter  is  decremented  and 
logging  is  NOT  performed. 


void  LogMotionData(double  XPIot,  double  YPIot){ 
if(LoggingData){ 

if((CycleCounter-  <=  0)  (NsxtPJoi  <  MaxDaiaF  oints)){ 
MotionData[NextPlot].XData  =  XPIot; 
MotionData[NextPlot].YData  =  YPIot; 

NextPlot++; 

CycleCounter  =  LoggingFrequency; 

} 

} 

} 


...................................................................... 

Function  DownLoadMotionData()  prompts  the  user  for  a  file  name  on  the  host 
system  to  store  the  data.  The  file  then  opened,  deleting  any  previous 
contents.  The  routine  is  dependent  on  the  “ytof”  call  in  the  yamabico 
account.  The  routine  then  transfers  character  string  representations  of  the 
recorded  data  to  the  Unix  host.  Afterwards,  the  function  ensures  that  the 
host  file  is  closed. 


void  DownLoadMotionData(){ 
char  FileName[35]; 

char  DataString[MAX_REAL_PRECISION]; 
int  LoopC-r'i  T.ter  ■  MaxDataPoints; 

MotionTraceDisable() ; 

PutStr(“tn\n\aReady  to  down  load  motion  data.  Connect  the"); 
PutStr(“vncable  and  press  any  key  to  continue."); 
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GetConsole(); 


PutStr("\a\n\n  Enter  the  name  of  the  Host  file  used  to  store  the  Motion  Data"); 
PutStrOn(WARNING:  PREVIOUS  FILE  CONTENTS  WILL  BE  DESTROYED)’); 
PutStr{“\n\n\lFile:  “); 

GetStr(FileName,35); 

PutStrHost(“ytof  “); 

PutStrHost(FileName) ; 

PutStrHost(“  w  \n"); 

PutStr(“\n\n\aReady  to  oown  load  data  to  Host  computer.  “); 

PutStr(AnPress  any  key  to  begin  “); 

GetConsoleO; 

PutStr(“\n\nDown  loading  data...”); 

for(LoopCounter=0;  LoopCounter<MaxDataPoints;  LoopCounter++){ 
PutStrHostfRtoAEfMotionDatajLoopCounterl.XData.DataString.S)); 
PutStrHost(“M"); 

PutStrHost(RtoAE(MotionData(LoopCounter].YDatatDataString>5)); 
PutStrriosi(“\n  ;; 

) 

PutHost(*\4’); 

PutHostC\4'); 

PutStr(“\aDown  load  complete"); 

} 


E.  WHEELS.H 

r . . . * . 

Author(s):  Scott  Book 
Project:  Yamabico  Robot  Control  System 
Date:  December  16, 1993 

Revised:  March  2, 1994 
File  Name:  wheels.h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  prototypes  to  available  wheel  sub-system 
routines.  This  set  of  prototypes  defines  the  wheel  sub-system 
interface. 

***** . * . . . . . * . I 


#ifndef _ WHEELSJH 

#define  WHEELS  H 


#include  “definitions.h' 


#define  MAXVELOCITY  60 


r 


Function  InitializeWheelsQ  initializes  the  wheels  subsystem  to  its  default 
settings. 


void  InKializeWheelsO; 
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r 


Function  WheelsEnable()  turns  on  the  motors  connected  to  the  wheels. 


void  WheelsEnable{); 
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Function  WheelsDisableQ  turns  off  the  motors  connected  to  the  wheels, 
void  WheelsDisable(); 


r 


Function  UpdateMovement()  updates  the  distance  traveled  by  both  wheels. 


void  UpdateMovement(); 
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r 


Function  GetDistanceTraveled()  returns  the  linear  distance  the  robot  has 
traveled  between  the  last  two  calls  to  UpdateMovement(). 


double  GetDistanceTraveledQ; 
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Function  GetOrientationChange()  returns  the  difference  between  the  changes 
in  the  distance  of  the  left  and  right  wheels  between  the  last  two  calls  to 
UpdateMovement() . 

...................................... . ......................... 


double  GetOrientationChangeO; 


Function  SetMovementO  translates  the  commanded  linear  and  rotational  vel¬ 
ocities  into  commanded  velocities  for  each  wheel. 

void  SetM  vement(double  LinearVelocity,  double  RotationalVelocity); 


#endif 
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F.  WHEELS.C 

r . ********** . . . ****** . ♦ . ......... 

Authors):  Scott  Book 
Project:  Yamabico  Robot  Control  System 
Date :  December  1 7, 1 993 

Revised:  March  2, 1994 
File  Name:  wheels.c 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 

Description:  This  file  defines  the  wheel  sub-system.  It  contains  the  hardware 

interface  routines  required  for  operation  with  the  Dual  Axis 

Controller.  The  file  is  seperated  into  two  section.  The  first 

is  the  private  section  containing  the  encapsulated  data  and 

functions.  The  second  section  is  the  public  section.  This 

section  defines  the  interface  routines. 

. * . * . . . . — ........ — **.*•**/ 


#include  “definitions.!!’ 
#include  “wheels. h" 


r 


PRIVATE  SECTION 

The  following  section  defines  the  encapsulated  definitions,  data  structures 
and  prototypes  used  in  the  system. 
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#define  RIGHT_DRIVE_PWM  OxfffffOOO 
#define  RIGHT_BRAKE_PWM  0xfffff002 
#define  IEFT_DRIVE_PWM  0xfffff004 
#define  LEFT_BRAKE_PWM  0xfffff006 

#define  MOTIONCONTROLADDRESS  0xfffff008 

#define  RIGHT_ENCODER_HIGH_WORD  OxfffffOlO 
#define  RIGHT_ENCODER_LOW_WORD  0xfffff012 
tdefine  LEFT_ENCODER_HIGH_WORD  0xfffff014 
#define  LEFT_ENCODER_LOW_WORD  0xfffff016 

#define  TREAD  52.41246478129945756620 

#define  TREAD_R  53.40092  /*  width  of  the  robot  (cm)  when  rotating.  */ 


#define  ENC_TO_DIST  0.00105570833333333333 


#define  KPWB  10.0 


r  Assumed  PWM  range:  0..127.  Set  at  90  to  allow  testing  on  weak  AC  generator.  */ 
r  Can  be  changed  to  127  if  generator  not  used  or  upgraded.  */ 

#define  PWMLIMIT  90 

static  int  MotorOn; 
static  BOOLEAN  Rotating; 

static  int  RightEncoderValue; 
static  int  LeftEncoderValue; 
static  int  OeltaRightEncoderValue; 
static  int  DeltaLeftEncoderValue; 

static  double  Rpwm; 
static  double  Lpwm; 
static  WORD  MotionControlWord; 


/***************************************************************************•**** 

The  following  static  function  declarations  are  the  prototypes  for  the 
encapsulated  functions. 

static  int  GetWheelEncoder(LONG  HighWord Address,  LONG  LowWordAddress); 
static  int  EncoderDifference(int  NewValue,  int  OldValue); 

static  int  GetPwm(double  CommandedVelocity,  double  Actual  Velocity,  double  Pwm); 
static  double  PwmLookUp(double  Velocity); 

static  void  SetMotorControl(int  Lpwm,  int  Rpwm,  int  MotionControlWord); 


Function  GetWheelEncoder()  appends  the  contents  of  the  shaft  encoder  low 
word  register  to  the  contents  of  the  high  word  register,  forming  a  long 
word.  It  then  returns  that  value. 

static  int  GetWheelEncoder(LONG  HighWordAddress,  LONG  LowWordAddress){ 
LONG  Wheel; 

Wheel  -  *(WORD*)HiqhWord Address; 

Wheel  *  Wheel  « 16; 

Wheel  +» *(WORD*)LowWordAddress; 

return  Wheel; 

} 
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r — * - * - ******* - * — ** — ** — * — **** 

Function  EncoderDifferenceO  returns  the  difference  between  the  new  shaft 
encoder  position  and  the  old  shaft  encoder  position.  The  shaft  encoder  values 
contain  only  24  bits  (OxOOOOOO-Oxffffff).  The  routine  adjusts  for  the  trans¬ 
ition  from  Oxffffff  to  0x000000  and  vice  versa. 


static  int  EncoderDifference(int  NewValue,  int  OldValue){ 
int  Difference  =  NewValue  -  Old  Value; 

if(Difference  <  -0x800000){ 

Difference  =  Difference  &  OxOOffffff; 

Jelse  if(Difference  >=  0x800000){ 

Difference  *  Difference  |  OxffOOOOOO; 

} 

return  Difference; 

} 


r . * . * . * . . . * — * . ***** 

Function  GetPwm()  returns  a  new  PWM  value  based  on  the  desired  velocity, 
the  actual  velocity  and  the  old  PWM  value. 

static  int  GetPwm(double  CommandedVelocity,  double  ActualVetocity,  double  Pwm){ 
double  a  =  0.7; 

int  PwmTemp  =  PwmLookUp(CommandedVetocity)  +  KPWB  * 
(CommandedVelocity  -  ActualVetocity); 

return  (a  *  PwmTemp  +  (1 .0  -  a)  *  Pwm); 

) 


Function  SetMotorControl()  sends  the  PWM  values  to  the  motor  control  board 
if  the  motors  are  turned  on  via  a  previous  call  to  MotorEnable().  If  the 
motors  are  not  turned  on,  then  a  command  to  free  the  motors  is  sent. 

static  void  SetMotorControl(int  Lpwm,  int  Rpwm,  int  MotionControlWord){ 
if(MotorOn){ 

*(WORD*)LEFT_DRIVE_PWM  -  (WORD)Lpwm; 

*(WORD*)RlGHT_DRIVE_PWM  -  (WORD)Rpwm; 

*(WORD*)MOTIONCONTROLADDRESS  =  (WORD)MotionControlWord; 
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}else{ 

*(WORD*)MOTIONCONTROLADDRESS  -  0x0303; 

} 

) 


r*** . * . * . . . ******** . * . * . * . . 

FUNCTION;  pwm_lookup 
PARAMETERS:  vel  (wheel  velocity) 

PURPOSE:  Determines  the  estimated  pwm  ratio  given 

the  desired  wheel  velocity  as  an  input.  (This  table  get  from  7.9  KHZ 
motor  output  curve). 

RETURNS:  pwm  value  based  upon  empirically  determined  velocity 
vs  pwm  ratio  curve. 

CALLED  BY:  control() 

CALLS:  none 

COMMENTS:  12  Jan  93  -  Dave  MacPherson,16  Sep  1993  changed  by  Ten-Min  Lee 
TASK:  Level  4  interrupt 

. . . . * . * . * . ***** . / 

static  double  PwmLookUp(double  Velocity)! 
double  v; 

double  pwm_value; 

v  =  Velocity; 
if  (v  ==  0.0  ) 
pwm_value  «=  0.0; 
else  if  (v  >=  0.0  &&  v  <  25.0) 
pwm_value  =  (0.96  *  v  +  49.0); 
else  if  (v  >=  25.0  &&  v  <  53.0) 
pwm_value  =  (0.82  *  (v  -  25.0)  +  73.0); 
else  if  (v  >=  53.0  &&  v  <*  65.0) 
pwm_value  «  (2.0  *  (v  -  53.0)  +  96.0); 
else  if  (v  >  65.0) 
pwm_value  *  127.0; 
else  if  (v  <  0.0  &&  v  >=  -  2.5) 
pwm_value  *  (1 .2  *  ( v )  -  54.0); 
else  if  (v  <  -2.5  &&  v  >=  -13.0) 
pwm_value  *  (0.76  *  (v  +  2.5)  -  57.0); 
else  if  (v  <  -13.0  &&  v  >*  -20.0) 
pwm_value  *  (0.43  *  (v  +  13.0)  -  65.0); 
else  if  (v  <  -20.0  &&  v  >=  -34.0) 
pwm_value  ■  (1 .0  *  (v  +  20.0)  -  68.0); 
else  if  (v  <  -34.0  &&  v  >*  -41 .0) 
pwm_value  «  (0.7  *  (v  +  34.0)  -  82.0); 
else  if  (v  <  -41 .0  &&  v  >«  -49.0) 
pwm_value  »  (1 .5  *  (v  +  41 .0)  -  87.0); 
else  if  (v  <  -49.0  &&  v  >=  -62.0) 
pwm_value  -  (1 .1  *  (v  +  49.0)  -  99.0); 


else  if  (v  <  -62.0  &&  v  >=  -65.0) 
pwm_value  =  (2.3  *  (v  +  62.0)  - 1 13.0); 
else 

pwm_value  *  -127.0; 

return  pwm_value; 

}  r  end  pwmjookup  */ 


r 


PUBLIC  SECTION 

The  following  section  defines  the  functions  that  provide  access  to  the 
wheel  sub-system. 
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r******************************************************************************* 
Function  Initialize WheelsO  initializes  all  of  the  private  global  variables 
in  this  module  to  the  desired  default  values. 


void  lnitializeWheels(){ 
WheelsDisableQ; 
Rotating  *  FALSE; 


Rpwm  *  0.0; 

Lpwm  =  0.0; 

MotionControlWord  -  0; 

DeltaRightEncoderValue  =  0; 

DeltaLeftEncoderValue  »  0; 

RightEncoderValue 

GetWheelEncoder(RIGHT_ENCODERJHIGH_WORD,RIGHT_ENCODER_LOW_WORD); 

LeftEncoderValue 

GetWheelEncoder(LEFT  ENCODER_HIGH_WORD,LEFT_ENCODER_LOW_WORD); 

} 


Function  WheelsEnable()  turns  on  the  motors  connected  to  the  wheels. 
This  is  done  by  setting  the  MotorOn  flag,  used  by  SetMotorControl(),  to 
a  non-zero  value. 


void  WheelsEnable(){ 


97 


MotorOn  *  1  ; 

) 


/*** . 

Function  WheelsDisableO  turns  off  the  motors  connected  to  the  wheels. 
This  is  done  by  setting  the  MotorOn  flag,  used  by  SetMotorControl(),  to 
zero. 

. . 7 

void  WheelsDisab!e(){ 

MotorOn  =  0; 

} 


Function  UpdateMovement{)  updates  the  distance  traveled  by  both  wheels.  It 
does  this  by  calculating  the  difference  between  the  current  shaft  encoder 
values  and  the  encoder  values  from  the  last  time  they  were  read.  It  also 
stores  the  current  encoder  values  which  will  be  used  as  the  last  encoder 
values  when  this  routine  is  called  again. 

void  UpdateMovement(}{ 
int  Wheel; 

Wheel 

GetWheelEncoder(RIGHT_ENCODER_HIGH_WORD,RIGHT_ENCODER__LOW_WORD); 
DeltaRightEncoderValue  =  EncoderDifference  (Wheel,  RightEncoderValue); 
RightEncoderValue  =  Wheel; 

r  The  left  motor  is  the  mirror  image  of  the  right.  Therefore  the  7 
r  amount  of  change  in  the  left  encoder  value  needs  to  be  negated  7 
r  to  show  the  proper  direction  of  rotation.  7 

Wheel 

GetWheelEncoder(LEFT_ENCODER_H!GH_WORD,l  EFT_ENCODERJLOW_WORD); 
DeltaLeftEncoderValue  =  -  EncoderDifference(Wheel,LeftEr»coderValue); 
LeftEncoderValue  -  Wheel; 

} 


Function  GetDistanceTraveled()  returns  the  linear  distance  the  robot  has 
traveled  between  the  last  two  calls  to  UpdateMovement().  It  makes  the 
calculations  based  on  the  measured  center  point  between  the  two  wheels  by 


taking  the  average  change  between  them. 
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double  GetDistanceTraveled(){ 
dc  jble  DistanceRight; 
double  DistanceLeft; 

DistanceRight  *  (double)DeltaRightEncoderValue  *  ENC_TO_DIST; 
DistanceLeft  *  (double)DeltaLeftEncoderValue  *  ENC_TO_DIST ; 

return  ((DistanceRight  +  DistanceLeft)  /  2.0); 

} 


r . * . 

Function  GetOrientationChange()  returns  the  difference  between  the  changes 
in  the  distance  of  the  left  and  right  wheels  between  the  last  two  calls  to 
UpdateMovement().  It  makes  the  calculations  based  on  the  center  point  between 
the  two  wheels.  MML10  made  a  distinction  based  on  whether  the  robot  was 
rotating  or  not  when  calculating  the  cer  er  point  between  the  wheels.  This 
distinction  is  included  in  this  routine. 


double  GetOrientationChange(){ 
double  DistanceRight; 
double  DistanceLeft; 
double  OrientationChange; 

DistanceRight  -  (double)DeltaRightEncoderValue  *  ENC_TO_DIST; 
DistanceLeft  -  (double)DeltaLeftEncoderValue  *  ENC_TO_DIST; 

if(Rotating){ 

OrientationChange  -  (DistanceRight  -  DistanceLeft)  /  TREAD_R; 
}else{ 

OrientationChange  *  (DistanceRight  -  DistanceLeft)  /  t  HEAD; 

} 

return  OrientationChange; 

) 


Function  SetMovemont()  translates  the  commanded  linear  and  rotational  vel¬ 
ocities  into  commanded  velocities  for  each  wheel.  It  then  calculates  the  PWM 
values  for  each  wheel  and  calls  SetMotorControl()  to  execute  PWM  commands. 
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void  SetMovement(doubie  LinearVelocity,  double  RotationalVelocity)} 
double  RightWheelVelocity; 
double  LeftWheelVelocity; 
double  Command  Right  Velocity; 
double  CommandLeftVelocity; 
double  Tread2; 
int  RpwmTemp; 
int  LpwmTemp; 

RightWheelVelocity  =  DeltaRightEncoderValue  *  ENC_TO_DIST  / 

MOTION_CONTROL_CYCLE; 

LeftWheelVelocity  =  DeltaLeftEncoderValue  *  ENC_TO_DIST  / 

MOTION_CONTROL_  CYCLE; 

if(Rotating){ 

Tread2  =  0.5  *  TREAD_R; 

}else{ 

Tread2  ^  0.5  *  TREAD; 

} 

CommandLeftVelocity  =  LinearVelocity  -  {Tread2  *  RotationalVelocity); 
CommandRightVelocty  =  LinearVelocity  +  (Tread2  *  RotationalVelocity); 

LpwmTemp  =  GetPwm(CommandLeftVelocity,  LeftWheelVelocity,  Lpwm); 

RpwmTemp  =  GetPwm(Com.  -iandRigmVetocity,  RightWheelVelocity,  Rpwm); 

Lpwm  ■  LpwmTemp; 

Rpwm  =  RpwmTemp; 

MotionControlWord  =  (MotionControlWord  &  OxfOfO)  | 

((LpwmTemp  •>  0)  ?  1  : 2)  | 

((RpwmTemp  >  0)  ?  OxIOC  :  0x200); 

if  (RpwmTemp  >  FWMLIMIT){ 

RpwmTemp  -  PWMLIMIT; 

}el$e  if(RpwmTemp  <  -PWMLIMIT){ 

RpwmTemp  -  -PWMLIMIT; 

) 

if(LpwmTemp  >  PWMLIMIT){ 

LpwmTemp  -  PWMLIMIT; 

}e!se  if(Lpwmfemp  <  -PWMLIMIT)( 

LpwmTemp  =  -PWMLIMIT; 

} 

SetMotorControl(LpwmTemp,  RpwmTemp,  MotionControlWord); 

} 


100 


APPENDIX  D 


A.  USER.H 

Author(s):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date:  December  8, 1993 

Revised:  March  4, 1994 

File  Name:  user.h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  prototype  for  USER().  This  is  not  required 
by  the  current  implementation  of  the  new  MML  system  since  user 
and  kernel  are  two  seperate  object  modules  that  are  loaded  at 
separate  locations.  However,  if  the  modules  are  combined  in  the 
future,  main()  can  call  USER()  directly  instead  of  user().  The 
prototype  would  then  be  required. 


#ifndef  _USER_H 
#define  USER  H 


......................................................................... 

Function  USER()  is  the  user  system  program  that  receives  control  from  main() 
after  the  robot's  sub-systems  have  been  initialized. 

void  USER(); 

#endif 


B.  USER.C 

r . 

Authors ):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date.  December8, 1993 

Revised:  March  4, 1994 

File  Name:  user.c 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  a  sample  user  program  that  can  be  used  with 
the  new  MML  system  created  using  ANSI  C.  The  purpose  of  this 
program  is  to  provide  an  interactive  menu  system  to  control  the 
robot  using  immediate  commands  only.  It  was  also  used  to  test 
the  implementation  of  the  immediate  commands.  Since  the  user 
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module  is  loaded  at  0x334000,  USER()  must  be  the  first  function 
defined. 


#include  “definitions.h" 
#include  “iosys.h” 
#include  “serial.h” 
#include  “motion.h" 
#include  “motiontrace.h" 
#include  “compatability.h" 
#include  “user.h" 


#define  ESC  0x1  b 


The  following  section  presents  the  prototypes  for  the  encapsulated  function(s) 
in  this  module. 


static  void  body(); 


r 


PUBLIC  SECTION 

The  following  section  defines  the  publicly  accessible  USER()  routine  that 
is  called  from  main(). 


7 


Function  USER()  is  essentially  a  shell  routine  that  calls  body().  The  reason 
for  this  approach  is  to  prevent  the  compiler  from  changing  the  starting 
address  of  USER()  by  storing  variables  and/or  strings  before  the  routine. 

. . . ** . * . / 

void  USER(){ 
body(); 

1 


r 


PRIVATE  SECTION 

The  following  section  defines  the  encapsulated  function  definition(s)  used 
within  this  module. 
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/ — *** — * — ** — * . ******** 

Function  body()  is  the  work  horse  routine  of  this  user  program,  tt  provides 
the  menu  system  to  interact  with  MML's  immediate  commands.  The  routine 
purposely  uses  the  older  MML-10  commad  calls  in  order  to  demonstrate  use  of 
the  compatability  macros  defined  in  compatability.h 
« . .  **  *‘*««*«*«***«“*«**«***“***««********«**«** . «***/ 

static  void  body<){ 
int  wait; 

char  KeyBoardHit; 

CONFIGURATION  VehicleConfiguration; 

KeyBoardHit  =  0; 

motion_on(); 

do{ 

PutStr(AnHit:  ESC  to  Terminate  Program"); 

PutStr(“\n  C  to  Change  Robot’s  Configuration  “); 

PutStr(An  R  to  Report  Robot's  Current  Configuration  “); 

PutStr(“\n  v  to  Change  Linear  Velocity"); 

PutStr(“\n  r  to  Change  Rotational  Velocity  “); 

PutStr(“\n  a  to  Change  Linear  Acceleration"); 

PutStr(“\n  b  to  Change  Rotational  Acceleration  “); 

PutStr(“\n  S  to  Change  the  Size  Constant  “); 

PutStr(“\n  s  to  Skip  the  next  Sequential  Instruction  “); 

PutStr(An  h  to  Halt  the  Robot  “); 

PutStr(“Vn  c  to  Resume  the  Robot  “); 

KeyBoardHit  -  GetConsole{); 

PutConsole(KeyBoardHit); 

switch(KeyBoardHit){ 
case  ESC  ; 
stop0(); 

PutStr(“\n\nThe  Total  Distance  Traveled  is;  “); 

PutReal(path_length(),4); 
break; 
case  ‘C’: 

cot_rob0(get Jnitial jposition()); 
break; 
case  ‘R’: 

get_robO(&VehicleConfiguration); 
repo  rt_configuration{VehicleConfigu  ration); 
break; 
case  V: 

PutStr(“\a\n\nEnter  Desired  Linear  Velocity;  “); 
speedO(GetReal()); 

MotionTraceEnable(3);  /*  Log  the  data  every  3  Motion  Control  Cycles.  7 


break; 
case  r: 

PutStr(Aa\n\nEnter  Desired  Rotational  Velocity:  “); 
r_speedO(GetReal()); 
break; 
case  ‘a’: 

PutStr(“\a\n\nEnter  Desired  Linear  Acceleration:  “); 
accO(GetReal()); 
break; 
case  ‘b’: 

PutStr{“\a\n\nEnter  Desired  Rotational  Acceleration:  “); 
r_accO(GetReal()); 
break; 
case  ‘S’: 

PutStr(“\a\n\nEnter  Desired  Size  Constant: 
size_const(GetReal{)) ; 
break; 
case  ‘s’: 
skip(); 
break; 
case  ‘h’: 
halt(); 
break; 
case  ‘c’: 
resume(); 
break; 
default: 
break; 

} 

}while(KeyBoardHit  1=  ESC); 

DownLoadMotionData() ; 

PutStr(“\n\nProgram  T  erminated.\a\n\n”); 

motion_off(); 

for(wait»0;  wait  <0x1 000;  wait++) 


} 


C.  COMPAT ABILITY .H 
r . — ***“ 

Authors):  Scott  Book 
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Project:  Yamabico  Robot  Control  System 
Date:  January  18, 1994 

Revise-1:  March  4, 1994 
File  Name:  compatability.h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  provides  macros  to  convert  old  MML-10  immediate 
command  calls  into  the  new  command  calls.  This  allows  user 
programs  created  for  MML-10  to  be  used  on  the  new  system. 

- ******/ 


#ifndef _ COMPATABILITY_H 

#define _ COMPATABILITY_H 

#include  “definitions.h” 

#include  “motion.h" 

#define  set_robO(P)  SetRobotConfiguration(P) 

#define  get_robO(P)  GetRobotConfiguration(P) 

#define  stop0()  Stop() 

#define  speedO(P)  SetLinearVelocity(P) 

#define  r_speed0(P)  SetRotationalVelocity(P) 

#define  accO(P)  SetLinearAcceleration(P) 

#define  r_accO(P)  SetRotationalAcceleration(P) 

#define  size_const(P)  SetSizeConstant(P) 

#define  path_length()  GetTotalDistance() 

#define  skip()  SkipPathElement() 

#define  halt()  HaltMotion() 

#define  resume()  ResumeMotion() 

Adeline  getjnitial _posltion()  SetlnitialPosition() 

#define  report_configuration(P)  ReportRobotConfiguration(P) 
#define  motion_on()  MotionOn{) 

#define  motion_off()  MotionOff() 
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#endrf 
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